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