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