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 soc_camera_host *ici = to_soc_camera_host(icd->dev.parent);
 156
 157         return v4l2_device_call_until_err(&ici->v4l2_dev, (__u32)icd, 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 /* Called with .vb_lock held */
 282 static int soc_camera_set_fmt(struct soc_camera_file *icf,
 283                               struct v4l2_format *f)
 284 {
 285         struct soc_camera_device *icd = icf->icd;
 286         struct soc_camera_host *ici = to_soc_camera_host(icd->dev.parent);
 287         struct v4l2_pix_format *pix = &f->fmt.pix;
 288         int ret;
 289
 290         /* We always call try_fmt() before set_fmt() or set_crop() */
 291         ret = ici->ops->try_fmt(icd, f);
 292         if (ret < 0)
 293                 return ret;
 294
 295         ret = ici->ops->set_fmt(icd, f);
 296         if (ret < 0) {
 297                 return ret;
 298         } else if (!icd->current_fmt ||
 299                    icd->current_fmt->fourcc != pix->pixelformat) {
 300                 dev_err(ici->v4l2_dev.dev,
 301                         "Host driver hasn't set up current format correctly!\n");
 302                 return -EINVAL;
 303         }
 304
 305         icd->rect_current.width         = pix->width;
 306         icd->rect_current.height        = pix->height;
 307         icf->vb_vidq.field              =
 308                 icd->field              = pix->field;
 309
 310         if (f->type != V4L2_BUF_TYPE_VIDEO_CAPTURE)
 311                 dev_warn(&icd->dev, "Attention! Wrong buf-type %d\n",
 312                          f->type);
 313
 314         dev_dbg(&icd->dev, "set width: %d height: %d\n",
 315                 icd->rect_current.width, icd->rect_current.height);
 316
 317         /* set physical bus parameters */
 318         return ici->ops->set_bus_param(icd, pix->pixelformat);
 319 }
 320
 321 static int soc_camera_open(struct file *file)
 322 {
 323         struct video_device *vdev = video_devdata(file);
 324         struct soc_camera_device *icd = container_of(vdev->parent, struct soc_camera_device, dev);
 325         struct soc_camera_link *icl = to_soc_camera_link(icd);
 326         struct soc_camera_host *ici;
 327         struct soc_camera_file *icf;
 328         int ret;
 329
 330         if (!icd->ops)
 331                 /* No device driver attached */
 332                 return -ENODEV;
 333
 334         ici = to_soc_camera_host(icd->dev.parent);
 335
 336         icf = vmalloc(sizeof(*icf));
 337         if (!icf)
 338                 return -ENOMEM;
 339
 340         if (!try_module_get(ici->ops->owner)) {
 341                 dev_err(&icd->dev, "Couldn't lock capture bus driver.\n");
 342                 ret = -EINVAL;
 343                 goto emgi;
 344         }
 345
 346         /* Protect against icd->ops->remove() until we module_get() both drivers. */
 347         mutex_lock(&icd->video_lock);
 348
 349         icf->icd = icd;
 350         icd->use_count++;
 351
 352         /* Now we really have to activate the camera */
 353         if (icd->use_count == 1) {
 354                 /* Restore parameters before the last close() per V4L2 API */
 355                 struct v4l2_format f = {
 356                         .type = V4L2_BUF_TYPE_VIDEO_CAPTURE,
 357                         .fmt.pix = {
 358                                 .width          = icd->rect_current.width,
 359                                 .height         = icd->rect_current.height,
 360                                 .field          = icd->field,
 361                                 .pixelformat    = icd->current_fmt->fourcc,
 362                                 .colorspace     = icd->current_fmt->colorspace,
 363                         },
 364                 };
 365
 366                 if (icl->power) {
 367                         ret = icl->power(icd->pdev, 1);
 368                         if (ret < 0)
 369                                 goto epower;
 370                 }
 371
 372                 /* The camera could have been already on, try to reset */
 373                 if (icl->reset)
 374                         icl->reset(icd->pdev);
 375
 376                 ret = ici->ops->add(icd);
 377                 if (ret < 0) {
 378                         dev_err(&icd->dev, "Couldn't activate the camera: %d\n", ret);
 379                         goto eiciadd;
 380                 }
 381
 382                 if (icd->ops->init) {
 383                         ret = icd->ops->init(icd);
 384                         if (ret < 0)
 385                                 goto einit;
 386                 }
 387
 388                 /* Try to configure with default parameters */
 389                 ret = soc_camera_set_fmt(icf, &f);
 390                 if (ret < 0)
 391                         goto esfmt;
 392         }
 393
 394         file->private_data = icf;
 395         dev_dbg(&icd->dev, "camera device open\n");
 396
 397         ici->ops->init_videobuf(&icf->vb_vidq, icd);
 398
 399         mutex_unlock(&icd->video_lock);
 400
 401         return 0;
 402
 403         /*
 404          * First five errors are entered with the .video_lock held
 405          * and use_count == 1
 406          */
 407 esfmt:
 408         if (icd->ops->release)
 409                 icd->ops->release(icd);
 410 einit:
 411         ici->ops->remove(icd);
 412 eiciadd:
 413         if (icl->power)
 414                 icl->power(icd->pdev, 0);
 415 epower:
 416         icd->use_count--;
 417         mutex_unlock(&icd->video_lock);
 418         module_put(ici->ops->owner);
 419 emgi:
 420         vfree(icf);
 421         return ret;
 422 }
 423
 424 static int soc_camera_close(struct file *file)
 425 {
 426         struct soc_camera_file *icf = file->private_data;
 427         struct soc_camera_device *icd = icf->icd;
 428         struct soc_camera_host *ici = to_soc_camera_host(icd->dev.parent);
 429         struct video_device *vdev = icd->vdev;
 430
 431         mutex_lock(&icd->video_lock);
 432         icd->use_count--;
 433         if (!icd->use_count) {
 434                 struct soc_camera_link *icl = to_soc_camera_link(icd);
 435
 436                 if (icd->ops->release)
 437                         icd->ops->release(icd);
 438                 ici->ops->remove(icd);
 439                 if (icl->power)
 440                         icl->power(icd->pdev, 0);
 441         }
 442
 443         mutex_unlock(&icd->video_lock);
 444
 445         module_put(ici->ops->owner);
 446
 447         vfree(icf);
 448
 449         dev_dbg(vdev->parent, "camera device close\n");
 450
 451         return 0;
 452 }
 453
 454 static ssize_t soc_camera_read(struct file *file, char __user *buf,
 455                                size_t count, loff_t *ppos)
 456 {
 457         struct soc_camera_file *icf = file->private_data;
 458         struct soc_camera_device *icd = icf->icd;
 459         struct video_device *vdev = icd->vdev;
 460         int err = -EINVAL;
 461
 462         dev_err(vdev->parent, "camera device read not implemented\n");
 463
 464         return err;
 465 }
 466
 467 static int soc_camera_mmap(struct file *file, struct vm_area_struct *vma)
 468 {
 469         struct soc_camera_file *icf = file->private_data;
 470         struct soc_camera_device *icd = icf->icd;
 471         int err;
 472
 473         dev_dbg(&icd->dev, "mmap called, vma=0x%08lx\n", (unsigned long)vma);
 474
 475         err = videobuf_mmap_mapper(&icf->vb_vidq, vma);
 476
 477         dev_dbg(&icd->dev, "vma start=0x%08lx, size=%ld, ret=%d\n",
 478                 (unsigned long)vma->vm_start,
 479                 (unsigned long)vma->vm_end - (unsigned long)vma->vm_start,
 480                 err);
 481
 482         return err;
 483 }
 484
 485 static unsigned int soc_camera_poll(struct file *file, poll_table *pt)
 486 {
 487         struct soc_camera_file *icf = file->private_data;
 488         struct soc_camera_device *icd = icf->icd;
 489         struct soc_camera_host *ici = to_soc_camera_host(icd->dev.parent);
 490
 491         if (list_empty(&icf->vb_vidq.stream)) {
 492                 dev_err(&icd->dev, "Trying to poll with no queued buffers!\n");
 493                 return POLLERR;
 494         }
 495
 496         return ici->ops->poll(file, pt);
 497 }
 498
 499 static struct v4l2_file_operations soc_camera_fops = {
 500         .owner          = THIS_MODULE,
 501         .open           = soc_camera_open,
 502         .release        = soc_camera_close,
 503         .ioctl          = video_ioctl2,
 504         .read           = soc_camera_read,
 505         .mmap           = soc_camera_mmap,
 506         .poll           = soc_camera_poll,
 507 };
 508
 509 static int soc_camera_s_fmt_vid_cap(struct file *file, void *priv,
 510                                     struct v4l2_format *f)
 511 {
 512         struct soc_camera_file *icf = file->private_data;
 513         struct soc_camera_device *icd = icf->icd;
 514         int ret;
 515
 516         WARN_ON(priv != file->private_data);
 517
 518         mutex_lock(&icf->vb_vidq.vb_lock);
 519
 520         if (icf->vb_vidq.bufs[0]) {
 521                 dev_err(&icd->dev, "S_FMT denied: queue initialised\n");
 522                 ret = -EBUSY;
 523                 goto unlock;
 524         }
 525
 526         ret = soc_camera_set_fmt(icf, f);
 527
 528 unlock:
 529         mutex_unlock(&icf->vb_vidq.vb_lock);
 530
 531         return ret;
 532 }
 533
 534 static int soc_camera_enum_fmt_vid_cap(struct file *file, void  *priv,
 535                                        struct v4l2_fmtdesc *f)
 536 {
 537         struct soc_camera_file *icf = file->private_data;
 538         struct soc_camera_device *icd = icf->icd;
 539         const struct soc_camera_data_format *format;
 540
 541         WARN_ON(priv != file->private_data);
 542
 543         if (f->index >= icd->num_user_formats)
 544                 return -EINVAL;
 545
 546         format = icd->user_formats[f->index].host_fmt;
 547
 548         strlcpy(f->description, format->name, sizeof(f->description));
 549         f->pixelformat = format->fourcc;
 550         return 0;
 551 }
 552
 553 static int soc_camera_g_fmt_vid_cap(struct file *file, void *priv,
 554                                     struct v4l2_format *f)
 555 {
 556         struct soc_camera_file *icf = file->private_data;
 557         struct soc_camera_device *icd = icf->icd;
 558         struct v4l2_pix_format *pix = &f->fmt.pix;
 559
 560         WARN_ON(priv != file->private_data);
 561
 562         pix->width              = icd->rect_current.width;
 563         pix->height             = icd->rect_current.height;
 564         pix->field              = icf->vb_vidq.field;
 565         pix->pixelformat        = icd->current_fmt->fourcc;
 566         pix->bytesperline       = pix->width *
 567                 DIV_ROUND_UP(icd->current_fmt->depth, 8);
 568         pix->sizeimage          = pix->height * pix->bytesperline;
 569         dev_dbg(&icd->dev, "current_fmt->fourcc: 0x%08x\n",
 570                 icd->current_fmt->fourcc);
 571         return 0;
 572 }
 573
 574 static int soc_camera_querycap(struct file *file, void  *priv,
 575                                struct v4l2_capability *cap)
 576 {
 577         struct soc_camera_file *icf = file->private_data;
 578         struct soc_camera_device *icd = icf->icd;
 579         struct soc_camera_host *ici = to_soc_camera_host(icd->dev.parent);
 580
 581         WARN_ON(priv != file->private_data);
 582
 583         strlcpy(cap->driver, ici->drv_name, sizeof(cap->driver));
 584         return ici->ops->querycap(ici, cap);
 585 }
 586
 587 static int soc_camera_streamon(struct file *file, void *priv,
 588                                enum v4l2_buf_type i)
 589 {
 590         struct soc_camera_file *icf = file->private_data;
 591         struct soc_camera_device *icd = icf->icd;
 592         struct soc_camera_host *ici = to_soc_camera_host(icd->dev.parent);
 593         int ret;
 594
 595         WARN_ON(priv != file->private_data);
 596
 597         if (i != V4L2_BUF_TYPE_VIDEO_CAPTURE)
 598                 return -EINVAL;
 599
 600         mutex_lock(&icd->video_lock);
 601
 602         v4l2_device_call_until_err(&ici->v4l2_dev, (__u32)icd, video, s_stream, 1);
 603
 604         /* This calls buf_queue from host driver's videobuf_queue_ops */
 605         ret = videobuf_streamon(&icf->vb_vidq);
 606
 607         mutex_unlock(&icd->video_lock);
 608
 609         return ret;
 610 }
 611
 612 static int soc_camera_streamoff(struct file *file, void *priv,
 613                                 enum v4l2_buf_type i)
 614 {
 615         struct soc_camera_file *icf = file->private_data;
 616         struct soc_camera_device *icd = icf->icd;
 617         struct soc_camera_host *ici = to_soc_camera_host(icd->dev.parent);
 618
 619         WARN_ON(priv != file->private_data);
 620
 621         if (i != V4L2_BUF_TYPE_VIDEO_CAPTURE)
 622                 return -EINVAL;
 623
 624         mutex_lock(&icd->video_lock);
 625
 626         /* This calls buf_release from host driver's videobuf_queue_ops for all
 627          * remaining buffers. When the last buffer is freed, stop capture */
 628         videobuf_streamoff(&icf->vb_vidq);
 629
 630         v4l2_device_call_until_err(&ici->v4l2_dev, (__u32)icd, video, s_stream, 0);
 631
 632         mutex_unlock(&icd->video_lock);
 633
 634         return 0;
 635 }
 636
 637 static int soc_camera_queryctrl(struct file *file, void *priv,
 638                                 struct v4l2_queryctrl *qc)
 639 {
 640         struct soc_camera_file *icf = file->private_data;
 641         struct soc_camera_device *icd = icf->icd;
 642         struct soc_camera_host *ici = to_soc_camera_host(icd->dev.parent);
 643         int i;
 644
 645         WARN_ON(priv != file->private_data);
 646
 647         if (!qc->id)
 648                 return -EINVAL;
 649
 650         /* First check host controls */
 651         for (i = 0; i < ici->ops->num_controls; i++)
 652                 if (qc->id == ici->ops->controls[i].id) {
 653                         memcpy(qc, &(ici->ops->controls[i]),
 654                                 sizeof(*qc));
 655                         return 0;
 656                 }
 657
 658         /* Then device controls */
 659         for (i = 0; i < icd->ops->num_controls; i++)
 660                 if (qc->id == icd->ops->controls[i].id) {
 661                         memcpy(qc, &(icd->ops->controls[i]),
 662                                 sizeof(*qc));
 663                         return 0;
 664                 }
 665
 666         return -EINVAL;
 667 }
 668
 669 static int soc_camera_g_ctrl(struct file *file, void *priv,
 670                              struct v4l2_control *ctrl)
 671 {
 672         struct soc_camera_file *icf = file->private_data;
 673         struct soc_camera_device *icd = icf->icd;
 674         struct soc_camera_host *ici = to_soc_camera_host(icd->dev.parent);
 675         int ret;
 676
 677         WARN_ON(priv != file->private_data);
 678
 679         switch (ctrl->id) {
 680         case V4L2_CID_GAIN:
 681                 if (icd->gain == (unsigned short)~0)
 682                         return -EINVAL;
 683                 ctrl->value = icd->gain;
 684                 return 0;
 685         case V4L2_CID_EXPOSURE:
 686                 if (icd->exposure == (unsigned short)~0)
 687                         return -EINVAL;
 688                 ctrl->value = icd->exposure;
 689                 return 0;
 690         }
 691
 692         if (ici->ops->get_ctrl) {
 693                 ret = ici->ops->get_ctrl(icd, ctrl);
 694                 if (ret != -ENOIOCTLCMD)
 695                         return ret;
 696         }
 697
 698         return v4l2_device_call_until_err(&ici->v4l2_dev, (__u32)icd, core, g_ctrl, ctrl);
 699 }
 700
 701 static int soc_camera_s_ctrl(struct file *file, void *priv,
 702                              struct v4l2_control *ctrl)
 703 {
 704         struct soc_camera_file *icf = file->private_data;
 705         struct soc_camera_device *icd = icf->icd;
 706         struct soc_camera_host *ici = to_soc_camera_host(icd->dev.parent);
 707         int ret;
 708
 709         WARN_ON(priv != file->private_data);
 710
 711         if (ici->ops->set_ctrl) {
 712                 ret = ici->ops->set_ctrl(icd, ctrl);
 713                 if (ret != -ENOIOCTLCMD)
 714                         return ret;
 715         }
 716
 717         return v4l2_device_call_until_err(&ici->v4l2_dev, (__u32)icd, core, s_ctrl, ctrl);
 718 }
 719
 720 static int soc_camera_cropcap(struct file *file, void *fh,
 721                               struct v4l2_cropcap *a)
 722 {
 723         struct soc_camera_file *icf = file->private_data;
 724         struct soc_camera_device *icd = icf->icd;
 725
 726         a->type                         = V4L2_BUF_TYPE_VIDEO_CAPTURE;
 727         a->bounds                       = icd->rect_max;
 728         a->defrect.left                 = icd->rect_max.left;
 729         a->defrect.top                  = icd->rect_max.top;
 730         a->defrect.width                = DEFAULT_WIDTH;
 731         a->defrect.height               = DEFAULT_HEIGHT;
 732         a->pixelaspect.numerator        = 1;
 733         a->pixelaspect.denominator      = 1;
 734
 735         return 0;
 736 }
 737
 738 static int soc_camera_g_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
 744         a->type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
 745         a->c    = icd->rect_current;
 746
 747         return 0;
 748 }
 749
 750 /*
 751  * According to the V4L2 API, drivers shall not update the struct v4l2_crop
 752  * argument with the actual geometry, instead, the user shall use G_CROP to
 753  * retrieve it. However, we expect camera host and client drivers to update
 754  * the argument, which we then use internally, but do not return to the user.
 755  */
 756 static int soc_camera_s_crop(struct file *file, void *fh,
 757                              struct v4l2_crop *a)
 758 {
 759         struct soc_camera_file *icf = file->private_data;
 760         struct soc_camera_device *icd = icf->icd;
 761         struct soc_camera_host *ici = to_soc_camera_host(icd->dev.parent);
 762         struct v4l2_rect rect = a->c;
 763         int ret;
 764
 765         if (a->type != V4L2_BUF_TYPE_VIDEO_CAPTURE)
 766                 return -EINVAL;
 767
 768         /* Cropping is allowed during a running capture, guard consistency */
 769         mutex_lock(&icf->vb_vidq.vb_lock);
 770
 771         /* Prohibit window size change with initialised buffers */
 772         if (icf->vb_vidq.bufs[0] && (rect.width != icd->rect_current.width ||
 773                                      rect.height != icd->rect_current.height)) {
 774                 dev_err(&icd->dev,
 775                         "S_CROP denied: queue initialised and sizes differ\n");
 776                 ret = -EBUSY;
 777                 goto unlock;
 778         }
 779
 780         if (rect.width > icd->rect_max.width)
 781                 rect.width = icd->rect_max.width;
 782
 783         if (rect.width < icd->width_min)
 784                 rect.width = icd->width_min;
 785
 786         if (rect.height > icd->rect_max.height)
 787                 rect.height = icd->rect_max.height;
 788
 789         if (rect.height < icd->height_min)
 790                 rect.height = icd->height_min;
 791
 792         if (rect.width + rect.left > icd->rect_max.width + icd->rect_max.left)
 793                 rect.left = icd->rect_max.width + icd->rect_max.left -
 794                         rect.width;
 795
 796         if (rect.height + rect.top > icd->rect_max.height + icd->rect_max.top)
 797                 rect.top = icd->rect_max.height + icd->rect_max.top -
 798                         rect.height;
 799
 800         ret = ici->ops->set_crop(icd, a);
 801         if (!ret)
 802                 icd->rect_current = rect;
 803
 804 unlock:
 805         mutex_unlock(&icf->vb_vidq.vb_lock);
 806
 807         return ret;
 808 }
 809
 810 static int soc_camera_g_chip_ident(struct file *file, void *fh,
 811                                    struct v4l2_dbg_chip_ident *id)
 812 {
 813         struct soc_camera_file *icf = file->private_data;
 814         struct soc_camera_device *icd = icf->icd;
 815         struct soc_camera_host *ici = to_soc_camera_host(icd->dev.parent);
 816
 817         return v4l2_device_call_until_err(&ici->v4l2_dev, (__u32)icd, core, g_chip_ident, id);
 818 }
 819
 820 #ifdef CONFIG_VIDEO_ADV_DEBUG
 821 static int soc_camera_g_register(struct file *file, void *fh,
 822                                  struct v4l2_dbg_register *reg)
 823 {
 824         struct soc_camera_file *icf = file->private_data;
 825         struct soc_camera_device *icd = icf->icd;
 826         struct soc_camera_host *ici = to_soc_camera_host(icd->dev.parent);
 827
 828         return v4l2_device_call_until_err(&ici->v4l2_dev, (__u32)icd, core, g_register, reg);
 829 }
 830
 831 static int soc_camera_s_register(struct file *file, void *fh,
 832                                  struct v4l2_dbg_register *reg)
 833 {
 834         struct soc_camera_file *icf = file->private_data;
 835         struct soc_camera_device *icd = icf->icd;
 836         struct soc_camera_host *ici = to_soc_camera_host(icd->dev.parent);
 837
 838         return v4l2_device_call_until_err(&ici->v4l2_dev, (__u32)icd, core, s_register, reg);
 839 }
 840 #endif
 841
 842 /* So far this function cannot fail */
 843 static void scan_add_host(struct soc_camera_host *ici)
 844 {
 845         struct soc_camera_device *icd;
 846
 847         mutex_lock(&list_lock);
 848
 849         list_for_each_entry(icd, &devices, list) {
 850                 if (icd->iface == ici->nr) {
 851                         int ret;
 852                         icd->dev.parent = ici->v4l2_dev.dev;
 853                         dev_set_name(&icd->dev, "%u-%u", icd->iface,
 854                                      icd->devnum);
 855                         ret = device_register(&icd->dev);
 856                         if (ret < 0) {
 857                                 icd->dev.parent = NULL;
 858                                 dev_err(&icd->dev,
 859                                         "Cannot register device: %d\n", ret);
 860                         }
 861                 }
 862         }
 863
 864         mutex_unlock(&list_lock);
 865 }
 866
 867 #ifdef CONFIG_I2C_BOARDINFO
 868 static int soc_camera_init_i2c(struct soc_camera_device *icd,
 869                                struct soc_camera_link *icl)
 870 {
 871         struct i2c_client *client;
 872         struct soc_camera_host *ici = to_soc_camera_host(icd->dev.parent);
 873         struct i2c_adapter *adap = i2c_get_adapter(icl->i2c_adapter_id);
 874         struct v4l2_subdev *subdev;
 875         int ret;
 876
 877         if (!adap) {
 878                 ret = -ENODEV;
 879                 dev_err(&icd->dev, "Cannot get I2C adapter #%d. No driver?\n",
 880                         icl->i2c_adapter_id);
 881                 goto ei2cga;
 882         }
 883
 884         icl->board_info->platform_data = icd;
 885
 886         subdev = v4l2_i2c_new_subdev_board(&ici->v4l2_dev, adap,
 887                                 icl->module_name, icl->board_info, NULL);
 888         if (!subdev) {
 889                 ret = -ENOMEM;
 890                 goto ei2cnd;
 891         }
 892
 893         subdev->grp_id = (__u32)icd;
 894         client = subdev->priv;
 895
 896         /* Use to_i2c_client(dev) to recover the i2c client */
 897         dev_set_drvdata(&icd->dev, &client->dev);
 898
 899         return 0;
 900 ei2cnd:
 901         i2c_put_adapter(adap);
 902 ei2cga:
 903         return ret;
 904 }
 905
 906 static void soc_camera_free_i2c(struct soc_camera_device *icd)
 907 {
 908         struct i2c_client *client =
 909                 to_i2c_client(to_soc_camera_control(icd));
 910         dev_set_drvdata(&icd->dev, NULL);
 911         v4l2_device_unregister_subdev(i2c_get_clientdata(client));
 912         i2c_unregister_device(client);
 913         i2c_put_adapter(client->adapter);
 914 }
 915 #else
 916 #define soc_camera_init_i2c(icd, icl)   (-ENODEV)
 917 #define soc_camera_free_i2c(icd)        do {} while (0)
 918 #endif
 919
 920 static int soc_camera_video_start(struct soc_camera_device *icd);
 921 static int video_dev_create(struct soc_camera_device *icd);
 922 /* Called during host-driver probe */
 923 static int soc_camera_probe(struct device *dev)
 924 {
 925         struct soc_camera_device *icd = to_soc_camera_dev(dev);
 926         struct soc_camera_host *ici = to_soc_camera_host(dev->parent);
 927         struct soc_camera_link *icl = to_soc_camera_link(icd);
 928         struct device *control = NULL;
 929         int ret;
 930
 931         dev_info(dev, "Probing %s\n", dev_name(dev));
 932
 933         if (icl->power) {
 934                 ret = icl->power(icd->pdev, 1);
 935                 if (ret < 0) {
 936                         dev_err(dev,
 937                                 "Platform failed to power-on the camera.\n");
 938                         goto epower;
 939                 }
 940         }
 941
 942         /* The camera could have been already on, try to reset */
 943         if (icl->reset)
 944                 icl->reset(icd->pdev);
 945
 946         ret = ici->ops->add(icd);
 947         if (ret < 0)
 948                 goto eadd;
 949
 950         /* Must have icd->vdev before registering the device */
 951         ret = video_dev_create(icd);
 952         if (ret < 0)
 953                 goto evdc;
 954
 955         /* Non-i2c cameras, e.g., soc_camera_platform, have no board_info */
 956         if (icl->board_info) {
 957                 ret = soc_camera_init_i2c(icd, icl);
 958                 if (ret < 0)
 959                         goto eadddev;
 960         } else if (!icl->add_device || !icl->del_device) {
 961                 ret = -EINVAL;
 962                 goto eadddev;
 963         } else {
 964                 if (icl->module_name)
 965                         ret = request_module(icl->module_name);
 966
 967                 ret = icl->add_device(icl, &icd->dev);
 968                 if (ret < 0)
 969                         goto eadddev;
 970
 971                 /* FIXME: this is racy, have to use driver-binding notification */
 972                 control = to_soc_camera_control(icd);
 973                 if (!control || !control->driver || !dev_get_drvdata(control) ||
 974                     !try_module_get(control->driver->owner)) {
 975                         icl->del_device(icl);
 976                         goto enodrv;
 977                 }
 978         }
 979
 980         /* At this point client .probe() should have run already */
 981         ret = soc_camera_init_user_formats(icd);
 982         if (ret < 0)
 983                 goto eiufmt;
 984
 985         icd->rect_current = icd->rect_max;
 986         icd->field = V4L2_FIELD_ANY;
 987
 988         /* ..._video_start() will create a device node, so we have to protect */
 989         mutex_lock(&icd->video_lock);
 990
 991         ret = soc_camera_video_start(icd);
 992         if (ret < 0)
 993                 goto evidstart;
 994
 995         /* Do we have to sysfs_remove_link() before device_unregister()? */
 996         if (to_soc_camera_control(icd) &&
 997             sysfs_create_link(&icd->dev.kobj, &to_soc_camera_control(icd)->kobj,
 998                               "control"))
 999                 dev_warn(&icd->dev, "Failed creating the control symlink\n");
1000
1001         ici->ops->remove(icd);
1002
1003         if (icl->power)
1004                 icl->power(icd->pdev, 0);
1005
1006         mutex_unlock(&icd->video_lock);
1007
1008         return 0;
1009
1010 evidstart:
1011         mutex_unlock(&icd->video_lock);
1012         soc_camera_free_user_formats(icd);
1013 eiufmt:
1014         if (icl->board_info) {
1015                 soc_camera_free_i2c(icd);
1016         } else {
1017                 icl->del_device(icl);
1018                 module_put(control->driver->owner);
1019         }
1020 enodrv:
1021 eadddev:
1022         video_device_release(icd->vdev);
1023 evdc:
1024         ici->ops->remove(icd);
1025 eadd:
1026         if (icl->power)
1027                 icl->power(icd->pdev, 0);
1028 epower:
1029         return ret;
1030 }
1031
1032 /* This is called on device_unregister, which only means we have to disconnect
1033  * from the host, but not remove ourselves from the device list */
1034 static int soc_camera_remove(struct device *dev)
1035 {
1036         struct soc_camera_device *icd = to_soc_camera_dev(dev);
1037         struct soc_camera_link *icl = to_soc_camera_link(icd);
1038         struct video_device *vdev = icd->vdev;
1039
1040         BUG_ON(!dev->parent);
1041
1042         if (vdev) {
1043                 mutex_lock(&icd->video_lock);
1044                 video_unregister_device(vdev);
1045                 icd->vdev = NULL;
1046                 mutex_unlock(&icd->video_lock);
1047         }
1048
1049         if (icl->board_info) {
1050                 soc_camera_free_i2c(icd);
1051         } else {
1052                 struct device_driver *drv = to_soc_camera_control(icd) ?
1053                         to_soc_camera_control(icd)->driver : NULL;
1054                 if (drv) {
1055                         icl->del_device(icl);
1056                         module_put(drv->owner);
1057                 }
1058         }
1059         soc_camera_free_user_formats(icd);
1060
1061         return 0;
1062 }
1063
1064 static int soc_camera_suspend(struct device *dev, pm_message_t state)
1065 {
1066         struct soc_camera_device *icd = to_soc_camera_dev(dev);
1067         struct soc_camera_host *ici = to_soc_camera_host(icd->dev.parent);
1068         int ret = 0;
1069
1070         if (ici->ops->suspend)
1071                 ret = ici->ops->suspend(icd, state);
1072
1073         return ret;
1074 }
1075
1076 static int soc_camera_resume(struct device *dev)
1077 {
1078         struct soc_camera_device *icd = to_soc_camera_dev(dev);
1079         struct soc_camera_host *ici = to_soc_camera_host(icd->dev.parent);
1080         int ret = 0;
1081
1082         if (ici->ops->resume)
1083                 ret = ici->ops->resume(icd);
1084
1085         return ret;
1086 }
1087
1088 static struct bus_type soc_camera_bus_type = {
1089         .name           = "soc-camera",
1090         .probe          = soc_camera_probe,
1091         .remove         = soc_camera_remove,
1092         .suspend        = soc_camera_suspend,
1093         .resume         = soc_camera_resume,
1094 };
1095
1096 static struct device_driver ic_drv = {
1097         .name   = "camera",
1098         .bus    = &soc_camera_bus_type,
1099         .owner  = THIS_MODULE,
1100 };
1101
1102 static void dummy_release(struct device *dev)
1103 {
1104 }
1105
1106 int soc_camera_host_register(struct soc_camera_host *ici)
1107 {
1108         struct soc_camera_host *ix;
1109         int ret;
1110
1111         if (!ici || !ici->ops ||
1112             !ici->ops->try_fmt ||
1113             !ici->ops->set_fmt ||
1114             !ici->ops->set_crop ||
1115             !ici->ops->set_bus_param ||
1116             !ici->ops->querycap ||
1117             !ici->ops->init_videobuf ||
1118             !ici->ops->reqbufs ||
1119             !ici->ops->add ||
1120             !ici->ops->remove ||
1121             !ici->ops->poll ||
1122             !ici->v4l2_dev.dev)
1123                 return -EINVAL;
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                         /* Not before device_unregister(), .remove
1164                          * needs parent to call ici->ops->remove() */
1165                         icd->dev.parent = NULL;
1166
1167                         /* If the host module is loaded again, device_register()
1168                          * would complain "already initialised" */
1169                         memset(&icd->dev.kobj, 0, sizeof(icd->dev.kobj));
1170                 }
1171         }
1172
1173         mutex_unlock(&list_lock);
1174
1175         v4l2_device_unregister(&ici->v4l2_dev);
1176 }
1177 EXPORT_SYMBOL(soc_camera_host_unregister);
1178
1179 /* Image capture device */
1180 static int soc_camera_device_register(struct soc_camera_device *icd)
1181 {
1182         struct soc_camera_device *ix;
1183         int num = -1, i;
1184
1185         for (i = 0; i < 256 && num < 0; i++) {
1186                 num = i;
1187                 /* Check if this index is available on this interface */
1188                 list_for_each_entry(ix, &devices, list) {
1189                         if (ix->iface == icd->iface && ix->devnum == i) {
1190                                 num = -1;
1191                                 break;
1192                         }
1193                 }
1194         }
1195
1196         if (num < 0)
1197                 /* ok, we have 256 cameras on this host...
1198                  * man, stay reasonable... */
1199                 return -ENOMEM;
1200
1201         icd->devnum = num;
1202         icd->dev.bus = &soc_camera_bus_type;
1203
1204         icd->dev.release        = dummy_release;
1205         icd->use_count          = 0;
1206         icd->host_priv          = NULL;
1207         mutex_init(&icd->video_lock);
1208
1209         list_add_tail(&icd->list, &devices);
1210
1211         return 0;
1212 }
1213
1214 static void soc_camera_device_unregister(struct soc_camera_device *icd)
1215 {
1216         list_del(&icd->list);
1217 }
1218
1219 static const struct v4l2_ioctl_ops soc_camera_ioctl_ops = {
1220         .vidioc_querycap         = soc_camera_querycap,
1221         .vidioc_g_fmt_vid_cap    = soc_camera_g_fmt_vid_cap,
1222         .vidioc_enum_fmt_vid_cap = soc_camera_enum_fmt_vid_cap,
1223         .vidioc_s_fmt_vid_cap    = soc_camera_s_fmt_vid_cap,
1224         .vidioc_enum_input       = soc_camera_enum_input,
1225         .vidioc_g_input          = soc_camera_g_input,
1226         .vidioc_s_input          = soc_camera_s_input,
1227         .vidioc_s_std            = soc_camera_s_std,
1228         .vidioc_reqbufs          = soc_camera_reqbufs,
1229         .vidioc_try_fmt_vid_cap  = soc_camera_try_fmt_vid_cap,
1230         .vidioc_querybuf         = soc_camera_querybuf,
1231         .vidioc_qbuf             = soc_camera_qbuf,
1232         .vidioc_dqbuf            = soc_camera_dqbuf,
1233         .vidioc_streamon         = soc_camera_streamon,
1234         .vidioc_streamoff        = soc_camera_streamoff,
1235         .vidioc_queryctrl        = soc_camera_queryctrl,
1236         .vidioc_g_ctrl           = soc_camera_g_ctrl,
1237         .vidioc_s_ctrl           = soc_camera_s_ctrl,
1238         .vidioc_cropcap          = soc_camera_cropcap,
1239         .vidioc_g_crop           = soc_camera_g_crop,
1240         .vidioc_s_crop           = soc_camera_s_crop,
1241         .vidioc_g_chip_ident     = soc_camera_g_chip_ident,
1242 #ifdef CONFIG_VIDEO_ADV_DEBUG
1243         .vidioc_g_register       = soc_camera_g_register,
1244         .vidioc_s_register       = soc_camera_s_register,
1245 #endif
1246 };
1247
1248 static int video_dev_create(struct soc_camera_device *icd)
1249 {
1250         struct soc_camera_host *ici = to_soc_camera_host(icd->dev.parent);
1251         struct video_device *vdev = video_device_alloc();
1252
1253         if (!vdev)
1254                 return -ENOMEM;
1255
1256         strlcpy(vdev->name, ici->drv_name, sizeof(vdev->name));
1257
1258         vdev->parent            = &icd->dev;
1259         vdev->current_norm      = V4L2_STD_UNKNOWN;
1260         vdev->fops              = &soc_camera_fops;
1261         vdev->ioctl_ops         = &soc_camera_ioctl_ops;
1262         vdev->release           = video_device_release;
1263         vdev->minor             = -1;
1264         vdev->tvnorms           = V4L2_STD_UNKNOWN;
1265
1266         icd->vdev = vdev;
1267
1268         return 0;
1269 }
1270
1271 /*
1272  * Called from soc_camera_probe() above (with .video_lock held???)
1273  */
1274 static int soc_camera_video_start(struct soc_camera_device *icd)
1275 {
1276         const struct v4l2_queryctrl *qctrl;
1277         int ret;
1278
1279         if (!icd->dev.parent)
1280                 return -ENODEV;
1281
1282         if (!icd->ops ||
1283             !icd->ops->query_bus_param ||
1284             !icd->ops->set_bus_param)
1285                 return -EINVAL;
1286
1287         ret = video_register_device(icd->vdev, VFL_TYPE_GRABBER,
1288                                     icd->vdev->minor);
1289         if (ret < 0) {
1290                 dev_err(&icd->dev, "video_register_device failed: %d\n", ret);
1291                 return ret;
1292         }
1293
1294         qctrl = soc_camera_find_qctrl(icd->ops, V4L2_CID_GAIN);
1295         icd->gain = qctrl ? qctrl->default_value : (unsigned short)~0;
1296         qctrl = soc_camera_find_qctrl(icd->ops, V4L2_CID_EXPOSURE);
1297         icd->exposure = qctrl ? qctrl->default_value : (unsigned short)~0;
1298
1299         return 0;
1300 }
1301
1302 static int __devinit soc_camera_pdrv_probe(struct platform_device *pdev)
1303 {
1304         struct soc_camera_link *icl = pdev->dev.platform_data;
1305         struct soc_camera_device *icd;
1306         int ret;
1307
1308         if (!icl)
1309                 return -EINVAL;
1310
1311         icd = kzalloc(sizeof(*icd), GFP_KERNEL);
1312         if (!icd)
1313                 return -ENOMEM;
1314
1315         icd->iface = icl->bus_id;
1316         icd->pdev = &pdev->dev;
1317         platform_set_drvdata(pdev, icd);
1318         icd->dev.platform_data = icl;
1319
1320         ret = soc_camera_device_register(icd);
1321         if (ret < 0)
1322                 goto escdevreg;
1323
1324         return 0;
1325
1326 escdevreg:
1327         kfree(icd);
1328
1329         return ret;
1330 }
1331
1332 /* Only called on rmmod for each platform device, since they are not
1333  * hot-pluggable. Now we know, that all our users - hosts and devices have
1334  * been unloaded already */
1335 static int __devexit soc_camera_pdrv_remove(struct platform_device *pdev)
1336 {
1337         struct soc_camera_device *icd = platform_get_drvdata(pdev);
1338
1339         if (!icd)
1340                 return -EINVAL;
1341
1342         soc_camera_device_unregister(icd);
1343
1344         kfree(icd);
1345
1346         return 0;
1347 }
1348
1349 static struct platform_driver __refdata soc_camera_pdrv = {
1350         .remove  = __devexit_p(soc_camera_pdrv_remove),
1351         .driver  = {
1352                 .name   = "soc-camera-pdrv",
1353                 .owner  = THIS_MODULE,
1354         },
1355 };
1356
1357 static int __init soc_camera_init(void)
1358 {
1359         int ret = bus_register(&soc_camera_bus_type);
1360         if (ret)
1361                 return ret;
1362         ret = driver_register(&ic_drv);
1363         if (ret)
1364                 goto edrvr;
1365
1366         ret = platform_driver_probe(&soc_camera_pdrv, soc_camera_pdrv_probe);
1367         if (ret)
1368                 goto epdr;
1369
1370         return 0;
1371
1372 epdr:
1373         driver_unregister(&ic_drv);
1374 edrvr:
1375         bus_unregister(&soc_camera_bus_type);
1376         return ret;
1377 }
1378
1379 static void __exit soc_camera_exit(void)
1380 {
1381         platform_driver_unregister(&soc_camera_pdrv);
1382         driver_unregister(&ic_drv);
1383         bus_unregister(&soc_camera_bus_type);
1384 }
1385
1386 module_init(soc_camera_init);
1387 module_exit(soc_camera_exit);
1388
1389 MODULE_DESCRIPTION("Image capture bus driver");
1390 MODULE_AUTHOR("Guennadi Liakhovetski <kernel@pengutronix.de>");
1391 MODULE_LICENSE("GPL");
1392 MODULE_ALIAS("platform:soc-camera-pdrv");