]> git.openfabrics.org - ~shefty/rdma-dev.git/blob - drivers/media/video/soc_camera.c
[media] v4l: soc-camera: fix multiple simultaneous user case
[~shefty/rdma-dev.git] / drivers / media / video / soc_camera.c
1 /*
2  * camera image capture (abstract) bus driver
3  *
4  * Copyright (C) 2008, Guennadi Liakhovetski <kernel@pengutronix.de>
5  *
6  * This driver provides an interface between platform-specific camera
7  * busses and camera devices. It should be used if the camera is
8  * connected not over a "proper" bus like PCI or USB, but over a
9  * special bus, like, for example, the Quick Capture interface on PXA270
10  * SoCs. Later it should also be used for i.MX31 SoCs from Freescale.
11  * It can handle multiple cameras and / or multiple busses, which can
12  * be used, e.g., in stereo-vision applications.
13  *
14  * This program is free software; you can redistribute it and/or modify
15  * it under the terms of the GNU General Public License version 2 as
16  * published by the Free Software Foundation.
17  */
18
19 #include <linux/device.h>
20 #include <linux/err.h>
21 #include <linux/i2c.h>
22 #include <linux/init.h>
23 #include <linux/list.h>
24 #include <linux/mutex.h>
25 #include <linux/module.h>
26 #include <linux/platform_device.h>
27 #include <linux/slab.h>
28 #include <linux/pm_runtime.h>
29 #include <linux/vmalloc.h>
30
31 #include <media/soc_camera.h>
32 #include <media/v4l2-common.h>
33 #include <media/v4l2-ioctl.h>
34 #include <media/v4l2-dev.h>
35 #include <media/videobuf-core.h>
36 #include <media/soc_mediabus.h>
37
38 /* Default to VGA resolution */
39 #define DEFAULT_WIDTH   640
40 #define DEFAULT_HEIGHT  480
41
42 static LIST_HEAD(hosts);
43 static LIST_HEAD(devices);
44 static DEFINE_MUTEX(list_lock);         /* Protects the list of hosts */
45
46 const struct soc_camera_format_xlate *soc_camera_xlate_by_fourcc(
47         struct soc_camera_device *icd, unsigned int fourcc)
48 {
49         unsigned int i;
50
51         for (i = 0; i < icd->num_user_formats; i++)
52                 if (icd->user_formats[i].host_fmt->fourcc == fourcc)
53                         return icd->user_formats + i;
54         return NULL;
55 }
56 EXPORT_SYMBOL(soc_camera_xlate_by_fourcc);
57
58 /**
59  * soc_camera_apply_sensor_flags() - apply platform SOCAM_SENSOR_INVERT_* flags
60  * @icl:        camera platform parameters
61  * @flags:      flags to be inverted according to platform configuration
62  * @return:     resulting flags
63  */
64 unsigned long soc_camera_apply_sensor_flags(struct soc_camera_link *icl,
65                                             unsigned long flags)
66 {
67         unsigned long f;
68
69         /* If only one of the two polarities is supported, switch to the opposite */
70         if (icl->flags & SOCAM_SENSOR_INVERT_HSYNC) {
71                 f = flags & (SOCAM_HSYNC_ACTIVE_HIGH | SOCAM_HSYNC_ACTIVE_LOW);
72                 if (f == SOCAM_HSYNC_ACTIVE_HIGH || f == SOCAM_HSYNC_ACTIVE_LOW)
73                         flags ^= SOCAM_HSYNC_ACTIVE_HIGH | SOCAM_HSYNC_ACTIVE_LOW;
74         }
75
76         if (icl->flags & SOCAM_SENSOR_INVERT_VSYNC) {
77                 f = flags & (SOCAM_VSYNC_ACTIVE_HIGH | SOCAM_VSYNC_ACTIVE_LOW);
78                 if (f == SOCAM_VSYNC_ACTIVE_HIGH || f == SOCAM_VSYNC_ACTIVE_LOW)
79                         flags ^= SOCAM_VSYNC_ACTIVE_HIGH | SOCAM_VSYNC_ACTIVE_LOW;
80         }
81
82         if (icl->flags & SOCAM_SENSOR_INVERT_PCLK) {
83                 f = flags & (SOCAM_PCLK_SAMPLE_RISING | SOCAM_PCLK_SAMPLE_FALLING);
84                 if (f == SOCAM_PCLK_SAMPLE_RISING || f == SOCAM_PCLK_SAMPLE_FALLING)
85                         flags ^= SOCAM_PCLK_SAMPLE_RISING | SOCAM_PCLK_SAMPLE_FALLING;
86         }
87
88         return flags;
89 }
90 EXPORT_SYMBOL(soc_camera_apply_sensor_flags);
91
92 static int soc_camera_try_fmt_vid_cap(struct file *file, void *priv,
93                                       struct v4l2_format *f)
94 {
95         struct soc_camera_device *icd = file->private_data;
96         struct soc_camera_host *ici = to_soc_camera_host(icd->dev.parent);
97
98         WARN_ON(priv != file->private_data);
99
100         /* limit format to hardware capabilities */
101         return ici->ops->try_fmt(icd, f);
102 }
103
104 static int soc_camera_enum_input(struct file *file, void *priv,
105                                  struct v4l2_input *inp)
106 {
107         struct soc_camera_device *icd = file->private_data;
108         int ret = 0;
109
110         if (inp->index != 0)
111                 return -EINVAL;
112
113         if (icd->ops->enum_input)
114                 ret = icd->ops->enum_input(icd, inp);
115         else {
116                 /* default is camera */
117                 inp->type = V4L2_INPUT_TYPE_CAMERA;
118                 inp->std  = V4L2_STD_UNKNOWN;
119                 strcpy(inp->name, "Camera");
120         }
121
122         return ret;
123 }
124
125 static int soc_camera_g_input(struct file *file, void *priv, unsigned int *i)
126 {
127         *i = 0;
128
129         return 0;
130 }
131
132 static int soc_camera_s_input(struct file *file, void *priv, unsigned int i)
133 {
134         if (i > 0)
135                 return -EINVAL;
136
137         return 0;
138 }
139
140 static int soc_camera_s_std(struct file *file, void *priv, v4l2_std_id *a)
141 {
142         struct soc_camera_device *icd = file->private_data;
143         struct v4l2_subdev *sd = soc_camera_to_subdev(icd);
144
145         return v4l2_subdev_call(sd, core, s_std, *a);
146 }
147
148 static int soc_camera_reqbufs(struct file *file, void *priv,
149                               struct v4l2_requestbuffers *p)
150 {
151         int ret;
152         struct soc_camera_device *icd = file->private_data;
153         struct soc_camera_host *ici = to_soc_camera_host(icd->dev.parent);
154
155         WARN_ON(priv != file->private_data);
156
157         if (icd->streamer && icd->streamer != file)
158                 return -EBUSY;
159
160         ret = videobuf_reqbufs(&icd->vb_vidq, p);
161         if (ret < 0)
162                 return ret;
163
164         ret = ici->ops->reqbufs(icd, p);
165         if (!ret && !icd->streamer)
166                 icd->streamer = file;
167
168         return ret;
169 }
170
171 static int soc_camera_querybuf(struct file *file, void *priv,
172                                struct v4l2_buffer *p)
173 {
174         struct soc_camera_device *icd = file->private_data;
175
176         WARN_ON(priv != file->private_data);
177
178         return videobuf_querybuf(&icd->vb_vidq, p);
179 }
180
181 static int soc_camera_qbuf(struct file *file, void *priv,
182                            struct v4l2_buffer *p)
183 {
184         struct soc_camera_device *icd = file->private_data;
185
186         WARN_ON(priv != file->private_data);
187
188         if (icd->streamer != file)
189                 return -EBUSY;
190
191         return videobuf_qbuf(&icd->vb_vidq, p);
192 }
193
194 static int soc_camera_dqbuf(struct file *file, void *priv,
195                             struct v4l2_buffer *p)
196 {
197         struct soc_camera_device *icd = file->private_data;
198
199         WARN_ON(priv != file->private_data);
200
201         if (icd->streamer != file)
202                 return -EBUSY;
203
204         return videobuf_dqbuf(&icd->vb_vidq, p, file->f_flags & O_NONBLOCK);
205 }
206
207 /* Always entered with .video_lock held */
208 static int soc_camera_init_user_formats(struct soc_camera_device *icd)
209 {
210         struct v4l2_subdev *sd = soc_camera_to_subdev(icd);
211         struct soc_camera_host *ici = to_soc_camera_host(icd->dev.parent);
212         unsigned int i, fmts = 0, raw_fmts = 0;
213         int ret;
214         enum v4l2_mbus_pixelcode code;
215
216         while (!v4l2_subdev_call(sd, video, enum_mbus_fmt, raw_fmts, &code))
217                 raw_fmts++;
218
219         if (!ici->ops->get_formats)
220                 /*
221                  * Fallback mode - the host will have to serve all
222                  * sensor-provided formats one-to-one to the user
223                  */
224                 fmts = raw_fmts;
225         else
226                 /*
227                  * First pass - only count formats this host-sensor
228                  * configuration can provide
229                  */
230                 for (i = 0; i < raw_fmts; i++) {
231                         ret = ici->ops->get_formats(icd, i, NULL);
232                         if (ret < 0)
233                                 return ret;
234                         fmts += ret;
235                 }
236
237         if (!fmts)
238                 return -ENXIO;
239
240         icd->user_formats =
241                 vmalloc(fmts * sizeof(struct soc_camera_format_xlate));
242         if (!icd->user_formats)
243                 return -ENOMEM;
244
245         icd->num_user_formats = fmts;
246
247         dev_dbg(&icd->dev, "Found %d supported formats.\n", fmts);
248
249         /* Second pass - actually fill data formats */
250         fmts = 0;
251         for (i = 0; i < raw_fmts; i++)
252                 if (!ici->ops->get_formats) {
253                         v4l2_subdev_call(sd, video, enum_mbus_fmt, i, &code);
254                         icd->user_formats[i].host_fmt =
255                                 soc_mbus_get_fmtdesc(code);
256                         icd->user_formats[i].code = code;
257                 } else {
258                         ret = ici->ops->get_formats(icd, i,
259                                                     &icd->user_formats[fmts]);
260                         if (ret < 0)
261                                 goto egfmt;
262                         fmts += ret;
263                 }
264
265         icd->current_fmt = &icd->user_formats[0];
266
267         return 0;
268
269 egfmt:
270         icd->num_user_formats = 0;
271         vfree(icd->user_formats);
272         return ret;
273 }
274
275 /* Always entered with .video_lock held */
276 static void soc_camera_free_user_formats(struct soc_camera_device *icd)
277 {
278         struct soc_camera_host *ici = to_soc_camera_host(icd->dev.parent);
279
280         if (ici->ops->put_formats)
281                 ici->ops->put_formats(icd);
282         icd->current_fmt = NULL;
283         icd->num_user_formats = 0;
284         vfree(icd->user_formats);
285         icd->user_formats = NULL;
286 }
287
288 #define pixfmtstr(x) (x) & 0xff, ((x) >> 8) & 0xff, ((x) >> 16) & 0xff, \
289         ((x) >> 24) & 0xff
290
291 /* Called with .vb_lock held, or from the first open(2), see comment there */
292 static int soc_camera_set_fmt(struct soc_camera_device *icd,
293                               struct v4l2_format *f)
294 {
295         struct soc_camera_host *ici = to_soc_camera_host(icd->dev.parent);
296         struct v4l2_pix_format *pix = &f->fmt.pix;
297         int ret;
298
299         dev_dbg(&icd->dev, "S_FMT(%c%c%c%c, %ux%u)\n",
300                 pixfmtstr(pix->pixelformat), pix->width, pix->height);
301
302         /* We always call try_fmt() before set_fmt() or set_crop() */
303         ret = ici->ops->try_fmt(icd, f);
304         if (ret < 0)
305                 return ret;
306
307         ret = ici->ops->set_fmt(icd, f);
308         if (ret < 0) {
309                 return ret;
310         } else if (!icd->current_fmt ||
311                    icd->current_fmt->host_fmt->fourcc != pix->pixelformat) {
312                 dev_err(&icd->dev,
313                         "Host driver hasn't set up current format correctly!\n");
314                 return -EINVAL;
315         }
316
317         icd->user_width         = pix->width;
318         icd->user_height        = pix->height;
319         icd->colorspace         = pix->colorspace;
320         icd->vb_vidq.field      =
321                 icd->field      = pix->field;
322
323         if (f->type != V4L2_BUF_TYPE_VIDEO_CAPTURE)
324                 dev_warn(&icd->dev, "Attention! Wrong buf-type %d\n",
325                          f->type);
326
327         dev_dbg(&icd->dev, "set width: %d height: %d\n",
328                 icd->user_width, icd->user_height);
329
330         /* set physical bus parameters */
331         return ici->ops->set_bus_param(icd, pix->pixelformat);
332 }
333
334 static int soc_camera_open(struct file *file)
335 {
336         struct video_device *vdev = video_devdata(file);
337         struct soc_camera_device *icd = container_of(vdev->parent,
338                                                      struct soc_camera_device,
339                                                      dev);
340         struct soc_camera_link *icl = to_soc_camera_link(icd);
341         struct soc_camera_host *ici;
342         int ret;
343
344         if (!icd->ops)
345                 /* No device driver attached */
346                 return -ENODEV;
347
348         ici = to_soc_camera_host(icd->dev.parent);
349
350         if (!try_module_get(ici->ops->owner)) {
351                 dev_err(&icd->dev, "Couldn't lock capture bus driver.\n");
352                 return -EINVAL;
353         }
354
355         /*
356          * Protect against icd->ops->remove() until we module_get() both
357          * drivers.
358          */
359         mutex_lock(&icd->video_lock);
360
361         icd->use_count++;
362
363         /* Now we really have to activate the camera */
364         if (icd->use_count == 1) {
365                 /* Restore parameters before the last close() per V4L2 API */
366                 struct v4l2_format f = {
367                         .type = V4L2_BUF_TYPE_VIDEO_CAPTURE,
368                         .fmt.pix = {
369                                 .width          = icd->user_width,
370                                 .height         = icd->user_height,
371                                 .field          = icd->field,
372                                 .colorspace     = icd->colorspace,
373                                 .pixelformat    =
374                                         icd->current_fmt->host_fmt->fourcc,
375                         },
376                 };
377
378                 if (icl->power) {
379                         ret = icl->power(icd->pdev, 1);
380                         if (ret < 0)
381                                 goto epower;
382                 }
383
384                 /* The camera could have been already on, try to reset */
385                 if (icl->reset)
386                         icl->reset(icd->pdev);
387
388                 ret = ici->ops->add(icd);
389                 if (ret < 0) {
390                         dev_err(&icd->dev, "Couldn't activate the camera: %d\n", ret);
391                         goto eiciadd;
392                 }
393
394                 pm_runtime_enable(&icd->vdev->dev);
395                 ret = pm_runtime_resume(&icd->vdev->dev);
396                 if (ret < 0 && ret != -ENOSYS)
397                         goto eresume;
398
399                 /*
400                  * Try to configure with default parameters. Notice: this is the
401                  * very first open, so, we cannot race against other calls,
402                  * apart from someone else calling open() simultaneously, but
403                  * .video_lock is protecting us against it.
404                  */
405                 ret = soc_camera_set_fmt(icd, &f);
406                 if (ret < 0)
407                         goto esfmt;
408
409                 ici->ops->init_videobuf(&icd->vb_vidq, icd);
410         }
411
412         file->private_data = icd;
413         dev_dbg(&icd->dev, "camera device open\n");
414
415         mutex_unlock(&icd->video_lock);
416
417         return 0;
418
419         /*
420          * First four errors are entered with the .video_lock held
421          * and use_count == 1
422          */
423 esfmt:
424         pm_runtime_disable(&icd->vdev->dev);
425 eresume:
426         ici->ops->remove(icd);
427 eiciadd:
428         if (icl->power)
429                 icl->power(icd->pdev, 0);
430 epower:
431         icd->use_count--;
432         mutex_unlock(&icd->video_lock);
433         module_put(ici->ops->owner);
434
435         return ret;
436 }
437
438 static int soc_camera_close(struct file *file)
439 {
440         struct soc_camera_device *icd = file->private_data;
441         struct soc_camera_host *ici = to_soc_camera_host(icd->dev.parent);
442
443         mutex_lock(&icd->video_lock);
444         icd->use_count--;
445         if (!icd->use_count) {
446                 struct soc_camera_link *icl = to_soc_camera_link(icd);
447
448                 pm_runtime_suspend(&icd->vdev->dev);
449                 pm_runtime_disable(&icd->vdev->dev);
450
451                 ici->ops->remove(icd);
452
453                 if (icl->power)
454                         icl->power(icd->pdev, 0);
455         }
456
457         if (icd->streamer == file)
458                 icd->streamer = NULL;
459
460         mutex_unlock(&icd->video_lock);
461
462         module_put(ici->ops->owner);
463
464         dev_dbg(&icd->dev, "camera device close\n");
465
466         return 0;
467 }
468
469 static ssize_t soc_camera_read(struct file *file, char __user *buf,
470                                size_t count, loff_t *ppos)
471 {
472         struct soc_camera_device *icd = file->private_data;
473         int err = -EINVAL;
474
475         dev_err(&icd->dev, "camera device read not implemented\n");
476
477         return err;
478 }
479
480 static int soc_camera_mmap(struct file *file, struct vm_area_struct *vma)
481 {
482         struct soc_camera_device *icd = file->private_data;
483         int err;
484
485         dev_dbg(&icd->dev, "mmap called, vma=0x%08lx\n", (unsigned long)vma);
486
487         if (icd->streamer != file)
488                 return -EBUSY;
489
490         err = videobuf_mmap_mapper(&icd->vb_vidq, vma);
491
492         dev_dbg(&icd->dev, "vma start=0x%08lx, size=%ld, ret=%d\n",
493                 (unsigned long)vma->vm_start,
494                 (unsigned long)vma->vm_end - (unsigned long)vma->vm_start,
495                 err);
496
497         return err;
498 }
499
500 static unsigned int soc_camera_poll(struct file *file, poll_table *pt)
501 {
502         struct soc_camera_device *icd = file->private_data;
503         struct soc_camera_host *ici = to_soc_camera_host(icd->dev.parent);
504
505         if (icd->streamer != file)
506                 return -EBUSY;
507
508         if (list_empty(&icd->vb_vidq.stream)) {
509                 dev_err(&icd->dev, "Trying to poll with no queued buffers!\n");
510                 return POLLERR;
511         }
512
513         return ici->ops->poll(file, pt);
514 }
515
516 static struct v4l2_file_operations soc_camera_fops = {
517         .owner          = THIS_MODULE,
518         .open           = soc_camera_open,
519         .release        = soc_camera_close,
520         .ioctl          = video_ioctl2,
521         .read           = soc_camera_read,
522         .mmap           = soc_camera_mmap,
523         .poll           = soc_camera_poll,
524 };
525
526 static int soc_camera_s_fmt_vid_cap(struct file *file, void *priv,
527                                     struct v4l2_format *f)
528 {
529         struct soc_camera_device *icd = file->private_data;
530         int ret;
531
532         WARN_ON(priv != file->private_data);
533
534         if (icd->streamer && icd->streamer != file)
535                 return -EBUSY;
536
537         mutex_lock(&icd->vb_vidq.vb_lock);
538
539         if (icd->vb_vidq.bufs[0]) {
540                 dev_err(&icd->dev, "S_FMT denied: queue initialised\n");
541                 ret = -EBUSY;
542                 goto unlock;
543         }
544
545         ret = soc_camera_set_fmt(icd, f);
546
547         if (!ret && !icd->streamer)
548                 icd->streamer = file;
549
550 unlock:
551         mutex_unlock(&icd->vb_vidq.vb_lock);
552
553         return ret;
554 }
555
556 static int soc_camera_enum_fmt_vid_cap(struct file *file, void  *priv,
557                                        struct v4l2_fmtdesc *f)
558 {
559         struct soc_camera_device *icd = file->private_data;
560         const struct soc_mbus_pixelfmt *format;
561
562         WARN_ON(priv != file->private_data);
563
564         if (f->index >= icd->num_user_formats)
565                 return -EINVAL;
566
567         format = icd->user_formats[f->index].host_fmt;
568
569         if (format->name)
570                 strlcpy(f->description, format->name, sizeof(f->description));
571         f->pixelformat = format->fourcc;
572         return 0;
573 }
574
575 static int soc_camera_g_fmt_vid_cap(struct file *file, void *priv,
576                                     struct v4l2_format *f)
577 {
578         struct soc_camera_device *icd = file->private_data;
579         struct v4l2_pix_format *pix = &f->fmt.pix;
580
581         WARN_ON(priv != file->private_data);
582
583         pix->width              = icd->user_width;
584         pix->height             = icd->user_height;
585         pix->field              = icd->vb_vidq.field;
586         pix->pixelformat        = icd->current_fmt->host_fmt->fourcc;
587         pix->bytesperline       = soc_mbus_bytes_per_line(pix->width,
588                                                 icd->current_fmt->host_fmt);
589         pix->colorspace         = icd->colorspace;
590         if (pix->bytesperline < 0)
591                 return pix->bytesperline;
592         pix->sizeimage          = pix->height * pix->bytesperline;
593         dev_dbg(&icd->dev, "current_fmt->fourcc: 0x%08x\n",
594                 icd->current_fmt->host_fmt->fourcc);
595         return 0;
596 }
597
598 static int soc_camera_querycap(struct file *file, void  *priv,
599                                struct v4l2_capability *cap)
600 {
601         struct soc_camera_device *icd = file->private_data;
602         struct soc_camera_host *ici = to_soc_camera_host(icd->dev.parent);
603
604         WARN_ON(priv != file->private_data);
605
606         strlcpy(cap->driver, ici->drv_name, sizeof(cap->driver));
607         return ici->ops->querycap(ici, cap);
608 }
609
610 static int soc_camera_streamon(struct file *file, void *priv,
611                                enum v4l2_buf_type i)
612 {
613         struct soc_camera_device *icd = file->private_data;
614         struct v4l2_subdev *sd = soc_camera_to_subdev(icd);
615         int ret;
616
617         WARN_ON(priv != file->private_data);
618
619         if (i != V4L2_BUF_TYPE_VIDEO_CAPTURE)
620                 return -EINVAL;
621
622         if (icd->streamer != file)
623                 return -EBUSY;
624
625         mutex_lock(&icd->video_lock);
626
627         v4l2_subdev_call(sd, video, s_stream, 1);
628
629         /* This calls buf_queue from host driver's videobuf_queue_ops */
630         ret = videobuf_streamon(&icd->vb_vidq);
631
632         mutex_unlock(&icd->video_lock);
633
634         return ret;
635 }
636
637 static int soc_camera_streamoff(struct file *file, void *priv,
638                                 enum v4l2_buf_type i)
639 {
640         struct soc_camera_device *icd = file->private_data;
641         struct v4l2_subdev *sd = soc_camera_to_subdev(icd);
642
643         WARN_ON(priv != file->private_data);
644
645         if (i != V4L2_BUF_TYPE_VIDEO_CAPTURE)
646                 return -EINVAL;
647
648         if (icd->streamer != file)
649                 return -EBUSY;
650
651         mutex_lock(&icd->video_lock);
652
653         /*
654          * This calls buf_release from host driver's videobuf_queue_ops for all
655          * remaining buffers. When the last buffer is freed, stop capture
656          */
657         videobuf_streamoff(&icd->vb_vidq);
658
659         v4l2_subdev_call(sd, video, s_stream, 0);
660
661         mutex_unlock(&icd->video_lock);
662
663         return 0;
664 }
665
666 static int soc_camera_queryctrl(struct file *file, void *priv,
667                                 struct v4l2_queryctrl *qc)
668 {
669         struct soc_camera_device *icd = file->private_data;
670         struct soc_camera_host *ici = to_soc_camera_host(icd->dev.parent);
671         int i;
672
673         WARN_ON(priv != file->private_data);
674
675         if (!qc->id)
676                 return -EINVAL;
677
678         /* First check host controls */
679         for (i = 0; i < ici->ops->num_controls; i++)
680                 if (qc->id == ici->ops->controls[i].id) {
681                         memcpy(qc, &(ici->ops->controls[i]),
682                                 sizeof(*qc));
683                         return 0;
684                 }
685
686         /* Then device controls */
687         for (i = 0; i < icd->ops->num_controls; i++)
688                 if (qc->id == icd->ops->controls[i].id) {
689                         memcpy(qc, &(icd->ops->controls[i]),
690                                 sizeof(*qc));
691                         return 0;
692                 }
693
694         return -EINVAL;
695 }
696
697 static int soc_camera_g_ctrl(struct file *file, void *priv,
698                              struct v4l2_control *ctrl)
699 {
700         struct soc_camera_device *icd = file->private_data;
701         struct soc_camera_host *ici = to_soc_camera_host(icd->dev.parent);
702         struct v4l2_subdev *sd = soc_camera_to_subdev(icd);
703         int ret;
704
705         WARN_ON(priv != file->private_data);
706
707         if (ici->ops->get_ctrl) {
708                 ret = ici->ops->get_ctrl(icd, ctrl);
709                 if (ret != -ENOIOCTLCMD)
710                         return ret;
711         }
712
713         return v4l2_subdev_call(sd, core, g_ctrl, ctrl);
714 }
715
716 static int soc_camera_s_ctrl(struct file *file, void *priv,
717                              struct v4l2_control *ctrl)
718 {
719         struct soc_camera_device *icd = file->private_data;
720         struct soc_camera_host *ici = to_soc_camera_host(icd->dev.parent);
721         struct v4l2_subdev *sd = soc_camera_to_subdev(icd);
722         int ret;
723
724         WARN_ON(priv != file->private_data);
725
726         if (ici->ops->set_ctrl) {
727                 ret = ici->ops->set_ctrl(icd, ctrl);
728                 if (ret != -ENOIOCTLCMD)
729                         return ret;
730         }
731
732         return v4l2_subdev_call(sd, core, s_ctrl, ctrl);
733 }
734
735 static int soc_camera_cropcap(struct file *file, void *fh,
736                               struct v4l2_cropcap *a)
737 {
738         struct soc_camera_device *icd = file->private_data;
739         struct soc_camera_host *ici = to_soc_camera_host(icd->dev.parent);
740
741         return ici->ops->cropcap(icd, a);
742 }
743
744 static int soc_camera_g_crop(struct file *file, void *fh,
745                              struct v4l2_crop *a)
746 {
747         struct soc_camera_device *icd = file->private_data;
748         struct soc_camera_host *ici = to_soc_camera_host(icd->dev.parent);
749         int ret;
750
751         mutex_lock(&icd->vb_vidq.vb_lock);
752         ret = ici->ops->get_crop(icd, a);
753         mutex_unlock(&icd->vb_vidq.vb_lock);
754
755         return ret;
756 }
757
758 /*
759  * According to the V4L2 API, drivers shall not update the struct v4l2_crop
760  * argument with the actual geometry, instead, the user shall use G_CROP to
761  * retrieve it.
762  */
763 static int soc_camera_s_crop(struct file *file, void *fh,
764                              struct v4l2_crop *a)
765 {
766         struct soc_camera_device *icd = file->private_data;
767         struct soc_camera_host *ici = to_soc_camera_host(icd->dev.parent);
768         struct v4l2_rect *rect = &a->c;
769         struct v4l2_crop current_crop;
770         int ret;
771
772         if (a->type != V4L2_BUF_TYPE_VIDEO_CAPTURE)
773                 return -EINVAL;
774
775         dev_dbg(&icd->dev, "S_CROP(%ux%u@%u:%u)\n",
776                 rect->width, rect->height, rect->left, rect->top);
777
778         /* Cropping is allowed during a running capture, guard consistency */
779         mutex_lock(&icd->vb_vidq.vb_lock);
780
781         /* If get_crop fails, we'll let host and / or client drivers decide */
782         ret = ici->ops->get_crop(icd, &current_crop);
783
784         /* Prohibit window size change with initialised buffers */
785         if (ret < 0) {
786                 dev_err(&icd->dev,
787                         "S_CROP denied: getting current crop failed\n");
788         } else if (icd->vb_vidq.bufs[0] &&
789                    (a->c.width != current_crop.c.width ||
790                     a->c.height != current_crop.c.height)) {
791                 dev_err(&icd->dev,
792                         "S_CROP denied: queue initialised and sizes differ\n");
793                 ret = -EBUSY;
794         } else {
795                 ret = ici->ops->set_crop(icd, a);
796         }
797
798         mutex_unlock(&icd->vb_vidq.vb_lock);
799
800         return ret;
801 }
802
803 static int soc_camera_g_parm(struct file *file, void *fh,
804                              struct v4l2_streamparm *a)
805 {
806         struct soc_camera_device *icd = file->private_data;
807         struct soc_camera_host *ici = to_soc_camera_host(icd->dev.parent);
808
809         if (ici->ops->get_parm)
810                 return ici->ops->get_parm(icd, a);
811
812         return -ENOIOCTLCMD;
813 }
814
815 static int soc_camera_s_parm(struct file *file, void *fh,
816                              struct v4l2_streamparm *a)
817 {
818         struct soc_camera_device *icd = file->private_data;
819         struct soc_camera_host *ici = to_soc_camera_host(icd->dev.parent);
820
821         if (ici->ops->set_parm)
822                 return ici->ops->set_parm(icd, a);
823
824         return -ENOIOCTLCMD;
825 }
826
827 static int soc_camera_g_chip_ident(struct file *file, void *fh,
828                                    struct v4l2_dbg_chip_ident *id)
829 {
830         struct soc_camera_device *icd = file->private_data;
831         struct v4l2_subdev *sd = soc_camera_to_subdev(icd);
832
833         return v4l2_subdev_call(sd, core, g_chip_ident, id);
834 }
835
836 #ifdef CONFIG_VIDEO_ADV_DEBUG
837 static int soc_camera_g_register(struct file *file, void *fh,
838                                  struct v4l2_dbg_register *reg)
839 {
840         struct soc_camera_device *icd = file->private_data;
841         struct v4l2_subdev *sd = soc_camera_to_subdev(icd);
842
843         return v4l2_subdev_call(sd, core, g_register, reg);
844 }
845
846 static int soc_camera_s_register(struct file *file, void *fh,
847                                  struct v4l2_dbg_register *reg)
848 {
849         struct soc_camera_device *icd = file->private_data;
850         struct v4l2_subdev *sd = soc_camera_to_subdev(icd);
851
852         return v4l2_subdev_call(sd, core, s_register, reg);
853 }
854 #endif
855
856 /* So far this function cannot fail */
857 static void scan_add_host(struct soc_camera_host *ici)
858 {
859         struct soc_camera_device *icd;
860
861         mutex_lock(&list_lock);
862
863         list_for_each_entry(icd, &devices, list) {
864                 if (icd->iface == ici->nr) {
865                         int ret;
866                         icd->dev.parent = ici->v4l2_dev.dev;
867                         dev_set_name(&icd->dev, "%u-%u", icd->iface,
868                                      icd->devnum);
869                         ret = device_register(&icd->dev);
870                         if (ret < 0) {
871                                 icd->dev.parent = NULL;
872                                 dev_err(&icd->dev,
873                                         "Cannot register device: %d\n", ret);
874                         }
875                 }
876         }
877
878         mutex_unlock(&list_lock);
879 }
880
881 #ifdef CONFIG_I2C_BOARDINFO
882 static int soc_camera_init_i2c(struct soc_camera_device *icd,
883                                struct soc_camera_link *icl)
884 {
885         struct i2c_client *client;
886         struct soc_camera_host *ici = to_soc_camera_host(icd->dev.parent);
887         struct i2c_adapter *adap = i2c_get_adapter(icl->i2c_adapter_id);
888         struct v4l2_subdev *subdev;
889
890         if (!adap) {
891                 dev_err(&icd->dev, "Cannot get I2C adapter #%d. No driver?\n",
892                         icl->i2c_adapter_id);
893                 goto ei2cga;
894         }
895
896         icl->board_info->platform_data = icd;
897
898         subdev = v4l2_i2c_new_subdev_board(&ici->v4l2_dev, adap,
899                                 icl->board_info, NULL);
900         if (!subdev)
901                 goto ei2cnd;
902
903         client = v4l2_get_subdevdata(subdev);
904
905         /* Use to_i2c_client(dev) to recover the i2c client */
906         dev_set_drvdata(&icd->dev, &client->dev);
907
908         return 0;
909 ei2cnd:
910         i2c_put_adapter(adap);
911 ei2cga:
912         return -ENODEV;
913 }
914
915 static void soc_camera_free_i2c(struct soc_camera_device *icd)
916 {
917         struct i2c_client *client =
918                 to_i2c_client(to_soc_camera_control(icd));
919         dev_set_drvdata(&icd->dev, NULL);
920         v4l2_device_unregister_subdev(i2c_get_clientdata(client));
921         i2c_unregister_device(client);
922         i2c_put_adapter(client->adapter);
923 }
924 #else
925 #define soc_camera_init_i2c(icd, icl)   (-ENODEV)
926 #define soc_camera_free_i2c(icd)        do {} while (0)
927 #endif
928
929 static int soc_camera_video_start(struct soc_camera_device *icd);
930 static int video_dev_create(struct soc_camera_device *icd);
931 /* Called during host-driver probe */
932 static int soc_camera_probe(struct device *dev)
933 {
934         struct soc_camera_device *icd = to_soc_camera_dev(dev);
935         struct soc_camera_host *ici = to_soc_camera_host(dev->parent);
936         struct soc_camera_link *icl = to_soc_camera_link(icd);
937         struct device *control = NULL;
938         struct v4l2_subdev *sd;
939         struct v4l2_mbus_framefmt mf;
940         int ret;
941
942         dev_info(dev, "Probing %s\n", dev_name(dev));
943
944         if (icl->power) {
945                 ret = icl->power(icd->pdev, 1);
946                 if (ret < 0) {
947                         dev_err(dev,
948                                 "Platform failed to power-on the camera.\n");
949                         goto epower;
950                 }
951         }
952
953         /* The camera could have been already on, try to reset */
954         if (icl->reset)
955                 icl->reset(icd->pdev);
956
957         ret = ici->ops->add(icd);
958         if (ret < 0)
959                 goto eadd;
960
961         /* Must have icd->vdev before registering the device */
962         ret = video_dev_create(icd);
963         if (ret < 0)
964                 goto evdc;
965
966         /* Non-i2c cameras, e.g., soc_camera_platform, have no board_info */
967         if (icl->board_info) {
968                 ret = soc_camera_init_i2c(icd, icl);
969                 if (ret < 0)
970                         goto eadddev;
971         } else if (!icl->add_device || !icl->del_device) {
972                 ret = -EINVAL;
973                 goto eadddev;
974         } else {
975                 if (icl->module_name)
976                         ret = request_module(icl->module_name);
977
978                 ret = icl->add_device(icl, &icd->dev);
979                 if (ret < 0)
980                         goto eadddev;
981
982                 /*
983                  * FIXME: this is racy, have to use driver-binding notification,
984                  * when it is available
985                  */
986                 control = to_soc_camera_control(icd);
987                 if (!control || !control->driver || !dev_get_drvdata(control) ||
988                     !try_module_get(control->driver->owner)) {
989                         icl->del_device(icl);
990                         goto enodrv;
991                 }
992         }
993
994         /* At this point client .probe() should have run already */
995         ret = soc_camera_init_user_formats(icd);
996         if (ret < 0)
997                 goto eiufmt;
998
999         icd->field = V4L2_FIELD_ANY;
1000
1001         /* ..._video_start() will create a device node, so we have to protect */
1002         mutex_lock(&icd->video_lock);
1003
1004         ret = soc_camera_video_start(icd);
1005         if (ret < 0)
1006                 goto evidstart;
1007
1008         /* Try to improve our guess of a reasonable window format */
1009         sd = soc_camera_to_subdev(icd);
1010         if (!v4l2_subdev_call(sd, video, g_mbus_fmt, &mf)) {
1011                 icd->user_width         = mf.width;
1012                 icd->user_height        = mf.height;
1013                 icd->colorspace         = mf.colorspace;
1014                 icd->field              = mf.field;
1015         }
1016
1017         /* Do we have to sysfs_remove_link() before device_unregister()? */
1018         if (sysfs_create_link(&icd->dev.kobj, &to_soc_camera_control(icd)->kobj,
1019                               "control"))
1020                 dev_warn(&icd->dev, "Failed creating the control symlink\n");
1021
1022         ici->ops->remove(icd);
1023
1024         if (icl->power)
1025                 icl->power(icd->pdev, 0);
1026
1027         mutex_unlock(&icd->video_lock);
1028
1029         return 0;
1030
1031 evidstart:
1032         mutex_unlock(&icd->video_lock);
1033         soc_camera_free_user_formats(icd);
1034 eiufmt:
1035         if (icl->board_info) {
1036                 soc_camera_free_i2c(icd);
1037         } else {
1038                 icl->del_device(icl);
1039                 module_put(control->driver->owner);
1040         }
1041 enodrv:
1042 eadddev:
1043         video_device_release(icd->vdev);
1044 evdc:
1045         ici->ops->remove(icd);
1046 eadd:
1047         if (icl->power)
1048                 icl->power(icd->pdev, 0);
1049 epower:
1050         return ret;
1051 }
1052
1053 /*
1054  * This is called on device_unregister, which only means we have to disconnect
1055  * from the host, but not remove ourselves from the device list
1056  */
1057 static int soc_camera_remove(struct device *dev)
1058 {
1059         struct soc_camera_device *icd = to_soc_camera_dev(dev);
1060         struct soc_camera_link *icl = to_soc_camera_link(icd);
1061         struct video_device *vdev = icd->vdev;
1062
1063         BUG_ON(!dev->parent);
1064
1065         if (vdev) {
1066                 mutex_lock(&icd->video_lock);
1067                 video_unregister_device(vdev);
1068                 icd->vdev = NULL;
1069                 mutex_unlock(&icd->video_lock);
1070         }
1071
1072         if (icl->board_info) {
1073                 soc_camera_free_i2c(icd);
1074         } else {
1075                 struct device_driver *drv = to_soc_camera_control(icd) ?
1076                         to_soc_camera_control(icd)->driver : NULL;
1077                 if (drv) {
1078                         icl->del_device(icl);
1079                         module_put(drv->owner);
1080                 }
1081         }
1082         soc_camera_free_user_formats(icd);
1083
1084         return 0;
1085 }
1086
1087 static int soc_camera_suspend(struct device *dev, pm_message_t state)
1088 {
1089         struct soc_camera_device *icd = to_soc_camera_dev(dev);
1090         struct soc_camera_host *ici = to_soc_camera_host(icd->dev.parent);
1091         int ret = 0;
1092
1093         if (ici->ops->suspend)
1094                 ret = ici->ops->suspend(icd, state);
1095
1096         return ret;
1097 }
1098
1099 static int soc_camera_resume(struct device *dev)
1100 {
1101         struct soc_camera_device *icd = to_soc_camera_dev(dev);
1102         struct soc_camera_host *ici = to_soc_camera_host(icd->dev.parent);
1103         int ret = 0;
1104
1105         if (ici->ops->resume)
1106                 ret = ici->ops->resume(icd);
1107
1108         return ret;
1109 }
1110
1111 struct bus_type soc_camera_bus_type = {
1112         .name           = "soc-camera",
1113         .probe          = soc_camera_probe,
1114         .remove         = soc_camera_remove,
1115         .suspend        = soc_camera_suspend,
1116         .resume         = soc_camera_resume,
1117 };
1118 EXPORT_SYMBOL_GPL(soc_camera_bus_type);
1119
1120 static struct device_driver ic_drv = {
1121         .name   = "camera",
1122         .bus    = &soc_camera_bus_type,
1123         .owner  = THIS_MODULE,
1124 };
1125
1126 static void dummy_release(struct device *dev)
1127 {
1128 }
1129
1130 static int default_cropcap(struct soc_camera_device *icd,
1131                            struct v4l2_cropcap *a)
1132 {
1133         struct v4l2_subdev *sd = soc_camera_to_subdev(icd);
1134         return v4l2_subdev_call(sd, video, cropcap, a);
1135 }
1136
1137 static int default_g_crop(struct soc_camera_device *icd, struct v4l2_crop *a)
1138 {
1139         struct v4l2_subdev *sd = soc_camera_to_subdev(icd);
1140         return v4l2_subdev_call(sd, video, g_crop, a);
1141 }
1142
1143 static int default_s_crop(struct soc_camera_device *icd, struct v4l2_crop *a)
1144 {
1145         struct v4l2_subdev *sd = soc_camera_to_subdev(icd);
1146         return v4l2_subdev_call(sd, video, s_crop, a);
1147 }
1148
1149 static int default_g_parm(struct soc_camera_device *icd,
1150                           struct v4l2_streamparm *parm)
1151 {
1152         struct v4l2_subdev *sd = soc_camera_to_subdev(icd);
1153         return v4l2_subdev_call(sd, video, g_parm, parm);
1154 }
1155
1156 static int default_s_parm(struct soc_camera_device *icd,
1157                           struct v4l2_streamparm *parm)
1158 {
1159         struct v4l2_subdev *sd = soc_camera_to_subdev(icd);
1160         return v4l2_subdev_call(sd, video, s_parm, parm);
1161 }
1162
1163 static void soc_camera_device_init(struct device *dev, void *pdata)
1164 {
1165         dev->platform_data      = pdata;
1166         dev->bus                = &soc_camera_bus_type;
1167         dev->release            = dummy_release;
1168 }
1169
1170 int soc_camera_host_register(struct soc_camera_host *ici)
1171 {
1172         struct soc_camera_host *ix;
1173         int ret;
1174
1175         if (!ici || !ici->ops ||
1176             !ici->ops->try_fmt ||
1177             !ici->ops->set_fmt ||
1178             !ici->ops->set_bus_param ||
1179             !ici->ops->querycap ||
1180             !ici->ops->init_videobuf ||
1181             !ici->ops->reqbufs ||
1182             !ici->ops->add ||
1183             !ici->ops->remove ||
1184             !ici->ops->poll ||
1185             !ici->v4l2_dev.dev)
1186                 return -EINVAL;
1187
1188         if (!ici->ops->set_crop)
1189                 ici->ops->set_crop = default_s_crop;
1190         if (!ici->ops->get_crop)
1191                 ici->ops->get_crop = default_g_crop;
1192         if (!ici->ops->cropcap)
1193                 ici->ops->cropcap = default_cropcap;
1194         if (!ici->ops->set_parm)
1195                 ici->ops->set_parm = default_s_parm;
1196         if (!ici->ops->get_parm)
1197                 ici->ops->get_parm = default_g_parm;
1198
1199         mutex_lock(&list_lock);
1200         list_for_each_entry(ix, &hosts, list) {
1201                 if (ix->nr == ici->nr) {
1202                         ret = -EBUSY;
1203                         goto edevreg;
1204                 }
1205         }
1206
1207         ret = v4l2_device_register(ici->v4l2_dev.dev, &ici->v4l2_dev);
1208         if (ret < 0)
1209                 goto edevreg;
1210
1211         list_add_tail(&ici->list, &hosts);
1212         mutex_unlock(&list_lock);
1213
1214         scan_add_host(ici);
1215
1216         return 0;
1217
1218 edevreg:
1219         mutex_unlock(&list_lock);
1220         return ret;
1221 }
1222 EXPORT_SYMBOL(soc_camera_host_register);
1223
1224 /* Unregister all clients! */
1225 void soc_camera_host_unregister(struct soc_camera_host *ici)
1226 {
1227         struct soc_camera_device *icd;
1228
1229         mutex_lock(&list_lock);
1230
1231         list_del(&ici->list);
1232
1233         list_for_each_entry(icd, &devices, list) {
1234                 if (icd->iface == ici->nr) {
1235                         void *pdata = icd->dev.platform_data;
1236                         /* The bus->remove will be called */
1237                         device_unregister(&icd->dev);
1238                         /*
1239                          * Not before device_unregister(), .remove
1240                          * needs parent to call ici->ops->remove().
1241                          * If the host module is loaded again, device_register()
1242                          * would complain "already initialised," since 2.6.32
1243                          * this is also needed to prevent use-after-free of the
1244                          * device private data.
1245                          */
1246                         memset(&icd->dev, 0, sizeof(icd->dev));
1247                         soc_camera_device_init(&icd->dev, pdata);
1248                 }
1249         }
1250
1251         mutex_unlock(&list_lock);
1252
1253         v4l2_device_unregister(&ici->v4l2_dev);
1254 }
1255 EXPORT_SYMBOL(soc_camera_host_unregister);
1256
1257 /* Image capture device */
1258 static int soc_camera_device_register(struct soc_camera_device *icd)
1259 {
1260         struct soc_camera_device *ix;
1261         int num = -1, i;
1262
1263         for (i = 0; i < 256 && num < 0; i++) {
1264                 num = i;
1265                 /* Check if this index is available on this interface */
1266                 list_for_each_entry(ix, &devices, list) {
1267                         if (ix->iface == icd->iface && ix->devnum == i) {
1268                                 num = -1;
1269                                 break;
1270                         }
1271                 }
1272         }
1273
1274         if (num < 0)
1275                 /*
1276                  * ok, we have 256 cameras on this host...
1277                  * man, stay reasonable...
1278                  */
1279                 return -ENOMEM;
1280
1281         icd->devnum             = num;
1282         icd->use_count          = 0;
1283         icd->host_priv          = NULL;
1284         mutex_init(&icd->video_lock);
1285
1286         list_add_tail(&icd->list, &devices);
1287
1288         return 0;
1289 }
1290
1291 static void soc_camera_device_unregister(struct soc_camera_device *icd)
1292 {
1293         list_del(&icd->list);
1294 }
1295
1296 static const struct v4l2_ioctl_ops soc_camera_ioctl_ops = {
1297         .vidioc_querycap         = soc_camera_querycap,
1298         .vidioc_g_fmt_vid_cap    = soc_camera_g_fmt_vid_cap,
1299         .vidioc_enum_fmt_vid_cap = soc_camera_enum_fmt_vid_cap,
1300         .vidioc_s_fmt_vid_cap    = soc_camera_s_fmt_vid_cap,
1301         .vidioc_enum_input       = soc_camera_enum_input,
1302         .vidioc_g_input          = soc_camera_g_input,
1303         .vidioc_s_input          = soc_camera_s_input,
1304         .vidioc_s_std            = soc_camera_s_std,
1305         .vidioc_reqbufs          = soc_camera_reqbufs,
1306         .vidioc_try_fmt_vid_cap  = soc_camera_try_fmt_vid_cap,
1307         .vidioc_querybuf         = soc_camera_querybuf,
1308         .vidioc_qbuf             = soc_camera_qbuf,
1309         .vidioc_dqbuf            = soc_camera_dqbuf,
1310         .vidioc_streamon         = soc_camera_streamon,
1311         .vidioc_streamoff        = soc_camera_streamoff,
1312         .vidioc_queryctrl        = soc_camera_queryctrl,
1313         .vidioc_g_ctrl           = soc_camera_g_ctrl,
1314         .vidioc_s_ctrl           = soc_camera_s_ctrl,
1315         .vidioc_cropcap          = soc_camera_cropcap,
1316         .vidioc_g_crop           = soc_camera_g_crop,
1317         .vidioc_s_crop           = soc_camera_s_crop,
1318         .vidioc_g_parm           = soc_camera_g_parm,
1319         .vidioc_s_parm           = soc_camera_s_parm,
1320         .vidioc_g_chip_ident     = soc_camera_g_chip_ident,
1321 #ifdef CONFIG_VIDEO_ADV_DEBUG
1322         .vidioc_g_register       = soc_camera_g_register,
1323         .vidioc_s_register       = soc_camera_s_register,
1324 #endif
1325 };
1326
1327 static int video_dev_create(struct soc_camera_device *icd)
1328 {
1329         struct soc_camera_host *ici = to_soc_camera_host(icd->dev.parent);
1330         struct video_device *vdev = video_device_alloc();
1331
1332         if (!vdev)
1333                 return -ENOMEM;
1334
1335         strlcpy(vdev->name, ici->drv_name, sizeof(vdev->name));
1336
1337         vdev->parent            = &icd->dev;
1338         vdev->current_norm      = V4L2_STD_UNKNOWN;
1339         vdev->fops              = &soc_camera_fops;
1340         vdev->ioctl_ops         = &soc_camera_ioctl_ops;
1341         vdev->release           = video_device_release;
1342         vdev->tvnorms           = V4L2_STD_UNKNOWN;
1343
1344         icd->vdev = vdev;
1345
1346         return 0;
1347 }
1348
1349 /*
1350  * Called from soc_camera_probe() above (with .video_lock held???)
1351  */
1352 static int soc_camera_video_start(struct soc_camera_device *icd)
1353 {
1354         struct device_type *type = icd->vdev->dev.type;
1355         int ret;
1356
1357         if (!icd->dev.parent)
1358                 return -ENODEV;
1359
1360         if (!icd->ops ||
1361             !icd->ops->query_bus_param ||
1362             !icd->ops->set_bus_param)
1363                 return -EINVAL;
1364
1365         ret = video_register_device(icd->vdev, VFL_TYPE_GRABBER, -1);
1366         if (ret < 0) {
1367                 dev_err(&icd->dev, "video_register_device failed: %d\n", ret);
1368                 return ret;
1369         }
1370
1371         /* Restore device type, possibly set by the subdevice driver */
1372         icd->vdev->dev.type = type;
1373
1374         return 0;
1375 }
1376
1377 static int __devinit soc_camera_pdrv_probe(struct platform_device *pdev)
1378 {
1379         struct soc_camera_link *icl = pdev->dev.platform_data;
1380         struct soc_camera_device *icd;
1381         int ret;
1382
1383         if (!icl)
1384                 return -EINVAL;
1385
1386         icd = kzalloc(sizeof(*icd), GFP_KERNEL);
1387         if (!icd)
1388                 return -ENOMEM;
1389
1390         icd->iface = icl->bus_id;
1391         icd->pdev = &pdev->dev;
1392         platform_set_drvdata(pdev, icd);
1393
1394         ret = soc_camera_device_register(icd);
1395         if (ret < 0)
1396                 goto escdevreg;
1397
1398         soc_camera_device_init(&icd->dev, icl);
1399
1400         icd->user_width         = DEFAULT_WIDTH;
1401         icd->user_height        = DEFAULT_HEIGHT;
1402
1403         return 0;
1404
1405 escdevreg:
1406         kfree(icd);
1407
1408         return ret;
1409 }
1410
1411 /*
1412  * Only called on rmmod for each platform device, since they are not
1413  * hot-pluggable. Now we know, that all our users - hosts and devices have
1414  * been unloaded already
1415  */
1416 static int __devexit soc_camera_pdrv_remove(struct platform_device *pdev)
1417 {
1418         struct soc_camera_device *icd = platform_get_drvdata(pdev);
1419
1420         if (!icd)
1421                 return -EINVAL;
1422
1423         soc_camera_device_unregister(icd);
1424
1425         kfree(icd);
1426
1427         return 0;
1428 }
1429
1430 static struct platform_driver __refdata soc_camera_pdrv = {
1431         .remove  = __devexit_p(soc_camera_pdrv_remove),
1432         .driver  = {
1433                 .name   = "soc-camera-pdrv",
1434                 .owner  = THIS_MODULE,
1435         },
1436 };
1437
1438 static int __init soc_camera_init(void)
1439 {
1440         int ret = bus_register(&soc_camera_bus_type);
1441         if (ret)
1442                 return ret;
1443         ret = driver_register(&ic_drv);
1444         if (ret)
1445                 goto edrvr;
1446
1447         ret = platform_driver_probe(&soc_camera_pdrv, soc_camera_pdrv_probe);
1448         if (ret)
1449                 goto epdr;
1450
1451         return 0;
1452
1453 epdr:
1454         driver_unregister(&ic_drv);
1455 edrvr:
1456         bus_unregister(&soc_camera_bus_type);
1457         return ret;
1458 }
1459
1460 static void __exit soc_camera_exit(void)
1461 {
1462         platform_driver_unregister(&soc_camera_pdrv);
1463         driver_unregister(&ic_drv);
1464         bus_unregister(&soc_camera_bus_type);
1465 }
1466
1467 module_init(soc_camera_init);
1468 module_exit(soc_camera_exit);
1469
1470 MODULE_DESCRIPTION("Image capture bus driver");
1471 MODULE_AUTHOR("Guennadi Liakhovetski <kernel@pengutronix.de>");
1472 MODULE_LICENSE("GPL");
1473 MODULE_ALIAS("platform:soc-camera-pdrv");