drivers/media/video/soc_camera.c

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