drivers/media/platform/via/via-camera.c

   1 // SPDX-License-Identifier: GPL-2.0-only
   2 /*
   3  * Driver for the VIA Chrome integrated camera controller.
   4  *
   5  * Copyright 2009,2010 Jonathan Corbet <corbet@lwn.net>
   6  *
   7  * This work was supported by the One Laptop Per Child project
   8  */
   9 #include <linux/kernel.h>
  10 #include <linux/module.h>
  11 #include <linux/device.h>
  12 #include <linux/list.h>
  13 #include <linux/pci.h>
  14 #include <linux/gpio/consumer.h>
  15 #include <linux/interrupt.h>
  16 #include <linux/platform_device.h>
  17 #include <linux/videodev2.h>
  18 #include <media/v4l2-device.h>
  19 #include <media/v4l2-ioctl.h>
  20 #include <media/v4l2-ctrls.h>
  21 #include <media/v4l2-event.h>
  22 #include <media/v4l2-image-sizes.h>
  23 #include <media/i2c/ov7670.h>
  24 #include <media/videobuf2-dma-sg.h>
  25 #include <linux/delay.h>
  26 #include <linux/dma-mapping.h>
  27 #include <linux/pm_qos.h>
  28 #include <linux/via-core.h>
  29 #include <linux/via_i2c.h>
  30
  31 #ifdef CONFIG_X86
  32 #include <asm/olpc.h>
  33 #else
  34 #define machine_is_olpc(x) 0
  35 #endif
  36
  37 #include "via-camera.h"
  38
  39 MODULE_ALIAS("platform:viafb-camera");
  40 MODULE_AUTHOR("Jonathan Corbet <corbet@lwn.net>");
  41 MODULE_DESCRIPTION("VIA framebuffer-based camera controller driver");
  42 MODULE_LICENSE("GPL");
  43
  44 static bool flip_image;
  45 module_param(flip_image, bool, 0444);
  46 MODULE_PARM_DESC(flip_image,
  47                 "If set, the sensor will be instructed to flip the image vertically.");
  48
  49 static bool override_serial;
  50 module_param(override_serial, bool, 0444);
  51 MODULE_PARM_DESC(override_serial,
  52                 "The camera driver will normally refuse to load if the XO 1.5 serial port is enabled.  Set this option to force-enable the camera.");
  53
  54 /*
  55  * The structure describing our camera.
  56  */
  57 enum viacam_opstate { S_IDLE = 0, S_RUNNING = 1 };
  58
  59 struct via_camera {
  60         struct v4l2_device v4l2_dev;
  61         struct v4l2_ctrl_handler ctrl_handler;
  62         struct video_device vdev;
  63         struct v4l2_subdev *sensor;
  64         struct platform_device *platdev;
  65         struct viafb_dev *viadev;
  66         struct mutex lock;
  67         enum viacam_opstate opstate;
  68         unsigned long flags;
  69         struct pm_qos_request qos_request;
  70         /*
  71          * GPIO info for power/reset management
  72          */
  73         struct gpio_desc *power_gpio;
  74         struct gpio_desc *reset_gpio;
  75         /*
  76          * I/O memory stuff.
  77          */
  78         void __iomem *mmio;     /* Where the registers live */
  79         void __iomem *fbmem;    /* Frame buffer memory */
  80         u32 fb_offset;          /* Reserved memory offset (FB) */
  81         /*
  82          * Capture buffers and related.  The controller supports
  83          * up to three, so that's what we have here.  These buffers
  84          * live in frame buffer memory, so we don't call them "DMA".
  85          */
  86         unsigned int cb_offsets[3];     /* offsets into fb mem */
  87         u8 __iomem *cb_addrs[3];        /* Kernel-space addresses */
  88         int n_cap_bufs;                 /* How many are we using? */
  89         struct vb2_queue vq;
  90         struct list_head buffer_queue;
  91         u32 sequence;
  92         /*
  93          * Video format information.  sensor_format is kept in a form
  94          * that we can use to pass to the sensor.  We always run the
  95          * sensor in VGA resolution, though, and let the controller
  96          * downscale things if need be.  So we keep the "real*
  97          * dimensions separately.
  98          */
  99         struct v4l2_pix_format sensor_format;
 100         struct v4l2_pix_format user_format;
 101         u32 mbus_code;
 102 };
 103
 104 /* buffer for one video frame */
 105 struct via_buffer {
 106         /* common v4l buffer stuff -- must be first */
 107         struct vb2_v4l2_buffer          vbuf;
 108         struct list_head                queue;
 109 };
 110
 111 /*
 112  * Yes, this is a hack, but there's only going to be one of these
 113  * on any system we know of.
 114  */
 115 static struct via_camera *via_cam_info;
 116
 117 /*
 118  * Flag values, manipulated with bitops
 119  */
 120 #define CF_DMA_ACTIVE    0      /* A frame is incoming */
 121 #define CF_CONFIG_NEEDED 1      /* Must configure hardware */
 122
 123
 124 /*
 125  * Nasty ugly v4l2 boilerplate.
 126  */
 127 #define sensor_call(cam, optype, func, args...) \
 128         v4l2_subdev_call(cam->sensor, optype, func, ##args)
 129
 130 /*
 131  * Debugging and related.
 132  */
 133 #define cam_err(cam, fmt, arg...) \
 134         dev_err(&(cam)->platdev->dev, fmt, ##arg)
 135 #define cam_warn(cam, fmt, arg...) \
 136         dev_warn(&(cam)->platdev->dev, fmt, ##arg)
 137 #define cam_dbg(cam, fmt, arg...) \
 138         dev_dbg(&(cam)->platdev->dev, fmt, ##arg)
 139
 140 /*
 141  * Format handling.  This is ripped almost directly from Hans's changes
 142  * to cafe_ccic.c.  It's a little unfortunate; until this change, we
 143  * didn't need to know anything about the format except its byte depth;
 144  * now this information must be managed at this level too.
 145  */
 146 static struct via_format {
 147         __u32 pixelformat;
 148         int bpp;   /* Bytes per pixel */
 149         u32 mbus_code;
 150 } via_formats[] = {
 151         {
 152                 .pixelformat    = V4L2_PIX_FMT_YUYV,
 153                 .mbus_code      = MEDIA_BUS_FMT_YUYV8_2X8,
 154                 .bpp            = 2,
 155         },
 156         /* RGB444 and Bayer should be doable, but have never been
 157            tested with this driver. RGB565 seems to work at the default
 158            resolution, but results in color corruption when being scaled by
 159            viacam_set_scaled(), and is disabled as a result. */
 160 };
 161 #define N_VIA_FMTS ARRAY_SIZE(via_formats)
 162
 163 static struct via_format *via_find_format(u32 pixelformat)
 164 {
 165         unsigned i;
 166
 167         for (i = 0; i < N_VIA_FMTS; i++)
 168                 if (via_formats[i].pixelformat == pixelformat)
 169                         return via_formats + i;
 170         /* Not found? Then return the first format. */
 171         return via_formats;
 172 }
 173
 174
 175 /*--------------------------------------------------------------------------*/
 176 /*
 177  * Sensor power/reset management.  This piece is OLPC-specific for
 178  * sure; other configurations will have things connected differently.
 179  */
 180 static int via_sensor_power_setup(struct via_camera *cam)
 181 {
 182         struct device *dev = &cam->platdev->dev;
 183
 184         cam->power_gpio = devm_gpiod_get(dev, "VGPIO3", GPIOD_OUT_LOW);
 185         if (IS_ERR(cam->power_gpio))
 186                 return dev_err_probe(dev, PTR_ERR(cam->power_gpio),
 187                                      "failed to get power GPIO");
 188
 189         /* Request the reset line asserted */
 190         cam->reset_gpio = devm_gpiod_get(dev, "VGPIO2", GPIOD_OUT_HIGH);
 191         if (IS_ERR(cam->reset_gpio))
 192                 return dev_err_probe(dev, PTR_ERR(cam->reset_gpio),
 193                                      "failed to get reset GPIO");
 194
 195         return 0;
 196 }
 197
 198 /*
 199  * Power up the sensor and perform the reset dance.
 200  */
 201 static void via_sensor_power_up(struct via_camera *cam)
 202 {
 203         gpiod_set_value(cam->power_gpio, 1);
 204         gpiod_set_value(cam->reset_gpio, 1);
 205         msleep(20);  /* Probably excessive */
 206         gpiod_set_value(cam->reset_gpio, 0);
 207         msleep(20);
 208 }
 209
 210 static void via_sensor_power_down(struct via_camera *cam)
 211 {
 212         gpiod_set_value(cam->power_gpio, 0);
 213         gpiod_set_value(cam->reset_gpio, 1);
 214 }
 215
 216
 217 static void via_sensor_power_release(struct via_camera *cam)
 218 {
 219         via_sensor_power_down(cam);
 220 }
 221
 222 /* --------------------------------------------------------------------------*/
 223 /* Sensor ops */
 224
 225 /*
 226  * Manage the ov7670 "flip" bit, which needs special help.
 227  */
 228 static int viacam_set_flip(struct via_camera *cam)
 229 {
 230         struct v4l2_control ctrl;
 231
 232         memset(&ctrl, 0, sizeof(ctrl));
 233         ctrl.id = V4L2_CID_VFLIP;
 234         ctrl.value = flip_image;
 235         return v4l2_s_ctrl(NULL, cam->sensor->ctrl_handler, &ctrl);
 236 }
 237
 238 /*
 239  * Configure the sensor.  It's up to the caller to ensure
 240  * that the camera is in the correct operating state.
 241  */
 242 static int viacam_configure_sensor(struct via_camera *cam)
 243 {
 244         struct v4l2_subdev_format format = {
 245                 .which = V4L2_SUBDEV_FORMAT_ACTIVE,
 246         };
 247         int ret;
 248
 249         v4l2_fill_mbus_format(&format.format, &cam->sensor_format, cam->mbus_code);
 250         ret = sensor_call(cam, core, init, 0);
 251         if (ret == 0)
 252                 ret = sensor_call(cam, pad, set_fmt, NULL, &format);
 253         /*
 254          * OV7670 does weird things if flip is set *before* format...
 255          */
 256         if (ret == 0)
 257                 ret = viacam_set_flip(cam);
 258         return ret;
 259 }
 260
 261
 262
 263 /* --------------------------------------------------------------------------*/
 264 /*
 265  * Some simple register accessors; they assume that the lock is held.
 266  *
 267  * Should we want to support the second capture engine, we could
 268  * hide the register difference by adding 0x1000 to registers in the
 269  * 0x300-350 range.
 270  */
 271 static inline void viacam_write_reg(struct via_camera *cam,
 272                 int reg, int value)
 273 {
 274         iowrite32(value, cam->mmio + reg);
 275 }
 276
 277 static inline int viacam_read_reg(struct via_camera *cam, int reg)
 278 {
 279         return ioread32(cam->mmio + reg);
 280 }
 281
 282 static inline void viacam_write_reg_mask(struct via_camera *cam,
 283                 int reg, int value, int mask)
 284 {
 285         int tmp = viacam_read_reg(cam, reg);
 286
 287         tmp = (tmp & ~mask) | (value & mask);
 288         viacam_write_reg(cam, reg, tmp);
 289 }
 290
 291
 292 /* --------------------------------------------------------------------------*/
 293 /* Interrupt management and handling */
 294
 295 static irqreturn_t viacam_quick_irq(int irq, void *data)
 296 {
 297         struct via_camera *cam = data;
 298         irqreturn_t ret = IRQ_NONE;
 299         int icv;
 300
 301         /*
 302          * All we do here is to clear the interrupts and tell
 303          * the handler thread to wake up.
 304          */
 305         spin_lock(&cam->viadev->reg_lock);
 306         icv = viacam_read_reg(cam, VCR_INTCTRL);
 307         if (icv & VCR_IC_EAV) {
 308                 icv |= VCR_IC_EAV|VCR_IC_EVBI|VCR_IC_FFULL;
 309                 viacam_write_reg(cam, VCR_INTCTRL, icv);
 310                 ret = IRQ_WAKE_THREAD;
 311         }
 312         spin_unlock(&cam->viadev->reg_lock);
 313         return ret;
 314 }
 315
 316 /*
 317  * Find the next buffer which has somebody waiting on it.
 318  */
 319 static struct via_buffer *viacam_next_buffer(struct via_camera *cam)
 320 {
 321         if (cam->opstate != S_RUNNING)
 322                 return NULL;
 323         if (list_empty(&cam->buffer_queue))
 324                 return NULL;
 325         return list_entry(cam->buffer_queue.next, struct via_buffer, queue);
 326 }
 327
 328 /*
 329  * The threaded IRQ handler.
 330  */
 331 static irqreturn_t viacam_irq(int irq, void *data)
 332 {
 333         struct via_camera *cam = data;
 334         struct via_buffer *vb;
 335         int bufn;
 336         struct sg_table *sgt;
 337
 338         mutex_lock(&cam->lock);
 339         /*
 340          * If there is no place to put the data frame, don't bother
 341          * with anything else.
 342          */
 343         vb = viacam_next_buffer(cam);
 344         if (vb == NULL)
 345                 goto done;
 346         /*
 347          * Figure out which buffer we just completed.
 348          */
 349         bufn = (viacam_read_reg(cam, VCR_INTCTRL) & VCR_IC_ACTBUF) >> 3;
 350         bufn -= 1;
 351         if (bufn < 0)
 352                 bufn = cam->n_cap_bufs - 1;
 353         /*
 354          * Copy over the data and let any waiters know.
 355          */
 356         sgt = vb2_dma_sg_plane_desc(&vb->vbuf.vb2_buf, 0);
 357         vb->vbuf.vb2_buf.timestamp = ktime_get_ns();
 358         viafb_dma_copy_out_sg(cam->cb_offsets[bufn], sgt->sgl, sgt->nents);
 359         vb->vbuf.sequence = cam->sequence++;
 360         vb->vbuf.field = V4L2_FIELD_NONE;
 361         list_del(&vb->queue);
 362         vb2_buffer_done(&vb->vbuf.vb2_buf, VB2_BUF_STATE_DONE);
 363 done:
 364         mutex_unlock(&cam->lock);
 365         return IRQ_HANDLED;
 366 }
 367
 368
 369 /*
 370  * These functions must mess around with the general interrupt
 371  * control register, which is relevant to much more than just the
 372  * camera.  Nothing else uses interrupts, though, as of this writing.
 373  * Should that situation change, we'll have to improve support at
 374  * the via-core level.
 375  */
 376 static void viacam_int_enable(struct via_camera *cam)
 377 {
 378         viacam_write_reg(cam, VCR_INTCTRL,
 379                         VCR_IC_INTEN|VCR_IC_EAV|VCR_IC_EVBI|VCR_IC_FFULL);
 380         viafb_irq_enable(VDE_I_C0AVEN);
 381 }
 382
 383 static void viacam_int_disable(struct via_camera *cam)
 384 {
 385         viafb_irq_disable(VDE_I_C0AVEN);
 386         viacam_write_reg(cam, VCR_INTCTRL, 0);
 387 }
 388
 389
 390
 391 /* --------------------------------------------------------------------------*/
 392 /* Controller operations */
 393
 394 /*
 395  * Set up our capture buffers in framebuffer memory.
 396  */
 397 static int viacam_ctlr_cbufs(struct via_camera *cam)
 398 {
 399         int nbuf = cam->viadev->camera_fbmem_size/cam->sensor_format.sizeimage;
 400         int i;
 401         unsigned int offset;
 402
 403         /*
 404          * See how many buffers we can work with.
 405          */
 406         if (nbuf >= 3) {
 407                 cam->n_cap_bufs = 3;
 408                 viacam_write_reg_mask(cam, VCR_CAPINTC, VCR_CI_3BUFS,
 409                                 VCR_CI_3BUFS);
 410         } else if (nbuf == 2) {
 411                 cam->n_cap_bufs = 2;
 412                 viacam_write_reg_mask(cam, VCR_CAPINTC, 0, VCR_CI_3BUFS);
 413         } else {
 414                 cam_warn(cam, "Insufficient frame buffer memory\n");
 415                 return -ENOMEM;
 416         }
 417         /*
 418          * Set them up.
 419          */
 420         offset = cam->fb_offset;
 421         for (i = 0; i < cam->n_cap_bufs; i++) {
 422                 cam->cb_offsets[i] = offset;
 423                 cam->cb_addrs[i] = cam->fbmem + offset;
 424                 viacam_write_reg(cam, VCR_VBUF1 + i*4, offset & VCR_VBUF_MASK);
 425                 offset += cam->sensor_format.sizeimage;
 426         }
 427         return 0;
 428 }
 429
 430 /*
 431  * Set the scaling register for downscaling the image.
 432  *
 433  * This register works like this...  Vertical scaling is enabled
 434  * by bit 26; if that bit is set, downscaling is controlled by the
 435  * value in bits 16:25.  Those bits are divided by 1024 to get
 436  * the scaling factor; setting just bit 25 thus cuts the height
 437  * in half.
 438  *
 439  * Horizontal scaling works about the same, but it's enabled by
 440  * bit 11, with bits 0:10 giving the numerator of a fraction
 441  * (over 2048) for the scaling value.
 442  *
 443  * This function is naive in that, if the user departs from
 444  * the 3x4 VGA scaling factor, the image will distort.  We
 445  * could work around that if it really seemed important.
 446  */
 447 static void viacam_set_scale(struct via_camera *cam)
 448 {
 449         unsigned int avscale;
 450         int sf;
 451
 452         if (cam->user_format.width == VGA_WIDTH)
 453                 avscale = 0;
 454         else {
 455                 sf = (cam->user_format.width*2048)/VGA_WIDTH;
 456                 avscale = VCR_AVS_HEN | sf;
 457         }
 458         if (cam->user_format.height < VGA_HEIGHT) {
 459                 sf = (1024*cam->user_format.height)/VGA_HEIGHT;
 460                 avscale |= VCR_AVS_VEN | (sf << 16);
 461         }
 462         viacam_write_reg(cam, VCR_AVSCALE, avscale);
 463 }
 464
 465
 466 /*
 467  * Configure image-related information into the capture engine.
 468  */
 469 static void viacam_ctlr_image(struct via_camera *cam)
 470 {
 471         int cicreg;
 472
 473         /*
 474          * Disable clock before messing with stuff - from the via
 475          * sample driver.
 476          */
 477         viacam_write_reg(cam, VCR_CAPINTC, ~(VCR_CI_ENABLE|VCR_CI_CLKEN));
 478         /*
 479          * Set up the controller for VGA resolution, modulo magic
 480          * offsets from the via sample driver.
 481          */
 482         viacam_write_reg(cam, VCR_HORRANGE, 0x06200120);
 483         viacam_write_reg(cam, VCR_VERTRANGE, 0x01de0000);
 484         viacam_set_scale(cam);
 485         /*
 486          * Image size info.
 487          */
 488         viacam_write_reg(cam, VCR_MAXDATA,
 489                         (cam->sensor_format.height << 16) |
 490                         (cam->sensor_format.bytesperline >> 3));
 491         viacam_write_reg(cam, VCR_MAXVBI, 0);
 492         viacam_write_reg(cam, VCR_VSTRIDE,
 493                         cam->user_format.bytesperline & VCR_VS_STRIDE);
 494         /*
 495          * Set up the capture interface control register,
 496          * everything but the "go" bit.
 497          *
 498          * The FIFO threshold is a bit of a magic number; 8 is what
 499          * VIA's sample code uses.
 500          */
 501         cicreg = VCR_CI_CLKEN |
 502                 0x08000000 |            /* FIFO threshold */
 503                 VCR_CI_FLDINV |         /* OLPC-specific? */
 504                 VCR_CI_VREFINV |        /* OLPC-specific? */
 505                 VCR_CI_DIBOTH |         /* Capture both fields */
 506                 VCR_CI_CCIR601_8;
 507         if (cam->n_cap_bufs == 3)
 508                 cicreg |= VCR_CI_3BUFS;
 509         /*
 510          * YUV formats need different byte swapping than RGB.
 511          */
 512         if (cam->user_format.pixelformat == V4L2_PIX_FMT_YUYV)
 513                 cicreg |= VCR_CI_YUYV;
 514         else
 515                 cicreg |= VCR_CI_UYVY;
 516         viacam_write_reg(cam, VCR_CAPINTC, cicreg);
 517 }
 518
 519
 520 static int viacam_config_controller(struct via_camera *cam)
 521 {
 522         int ret;
 523         unsigned long flags;
 524
 525         spin_lock_irqsave(&cam->viadev->reg_lock, flags);
 526         ret = viacam_ctlr_cbufs(cam);
 527         if (!ret)
 528                 viacam_ctlr_image(cam);
 529         spin_unlock_irqrestore(&cam->viadev->reg_lock, flags);
 530         clear_bit(CF_CONFIG_NEEDED, &cam->flags);
 531         return ret;
 532 }
 533
 534 /*
 535  * Make it start grabbing data.
 536  */
 537 static void viacam_start_engine(struct via_camera *cam)
 538 {
 539         spin_lock_irq(&cam->viadev->reg_lock);
 540         viacam_write_reg_mask(cam, VCR_CAPINTC, VCR_CI_ENABLE, VCR_CI_ENABLE);
 541         viacam_int_enable(cam);
 542         (void) viacam_read_reg(cam, VCR_CAPINTC); /* Force post */
 543         cam->opstate = S_RUNNING;
 544         spin_unlock_irq(&cam->viadev->reg_lock);
 545 }
 546
 547
 548 static void viacam_stop_engine(struct via_camera *cam)
 549 {
 550         spin_lock_irq(&cam->viadev->reg_lock);
 551         viacam_int_disable(cam);
 552         viacam_write_reg_mask(cam, VCR_CAPINTC, 0, VCR_CI_ENABLE);
 553         (void) viacam_read_reg(cam, VCR_CAPINTC); /* Force post */
 554         cam->opstate = S_IDLE;
 555         spin_unlock_irq(&cam->viadev->reg_lock);
 556 }
 557
 558
 559 /* --------------------------------------------------------------------------*/
 560 /* vb2 callback ops */
 561
 562 static struct via_buffer *vb2_to_via_buffer(struct vb2_buffer *vb)
 563 {
 564         struct vb2_v4l2_buffer *vbuf = to_vb2_v4l2_buffer(vb);
 565
 566         return container_of(vbuf, struct via_buffer, vbuf);
 567 }
 568
 569 static void viacam_vb2_queue(struct vb2_buffer *vb)
 570 {
 571         struct via_camera *cam = vb2_get_drv_priv(vb->vb2_queue);
 572         struct via_buffer *via = vb2_to_via_buffer(vb);
 573
 574         list_add_tail(&via->queue, &cam->buffer_queue);
 575 }
 576
 577 static int viacam_vb2_prepare(struct vb2_buffer *vb)
 578 {
 579         struct via_camera *cam = vb2_get_drv_priv(vb->vb2_queue);
 580
 581         if (vb2_plane_size(vb, 0) < cam->user_format.sizeimage) {
 582                 cam_dbg(cam,
 583                         "Plane size too small (%lu < %u)\n",
 584                         vb2_plane_size(vb, 0),
 585                         cam->user_format.sizeimage);
 586                 return -EINVAL;
 587         }
 588
 589         vb2_set_plane_payload(vb, 0, cam->user_format.sizeimage);
 590
 591         return 0;
 592 }
 593
 594 static int viacam_vb2_queue_setup(struct vb2_queue *vq,
 595                                   unsigned int *nbufs,
 596                                   unsigned int *num_planes, unsigned int sizes[],
 597                                   struct device *alloc_devs[])
 598 {
 599         struct via_camera *cam = vb2_get_drv_priv(vq);
 600         int size = cam->user_format.sizeimage;
 601
 602         if (*num_planes)
 603                 return sizes[0] < size ? -EINVAL : 0;
 604
 605         *num_planes = 1;
 606         sizes[0] = size;
 607         return 0;
 608 }
 609
 610 static int viacam_vb2_start_streaming(struct vb2_queue *vq, unsigned int count)
 611 {
 612         struct via_camera *cam = vb2_get_drv_priv(vq);
 613         struct via_buffer *buf, *tmp;
 614         int ret = 0;
 615
 616         if (cam->opstate != S_IDLE) {
 617                 ret = -EBUSY;
 618                 goto out;
 619         }
 620         /*
 621          * Configure things if need be.
 622          */
 623         if (test_bit(CF_CONFIG_NEEDED, &cam->flags)) {
 624                 ret = viacam_configure_sensor(cam);
 625                 if (ret)
 626                         goto out;
 627                 ret = viacam_config_controller(cam);
 628                 if (ret)
 629                         goto out;
 630         }
 631         cam->sequence = 0;
 632         /*
 633          * If the CPU goes into C3, the DMA transfer gets corrupted and
 634          * users start filing unsightly bug reports.  Put in a "latency"
 635          * requirement which will keep the CPU out of the deeper sleep
 636          * states.
 637          */
 638         cpu_latency_qos_add_request(&cam->qos_request, 50);
 639         viacam_start_engine(cam);
 640         return 0;
 641 out:
 642         list_for_each_entry_safe(buf, tmp, &cam->buffer_queue, queue) {
 643                 list_del(&buf->queue);
 644                 vb2_buffer_done(&buf->vbuf.vb2_buf, VB2_BUF_STATE_QUEUED);
 645         }
 646         return ret;
 647 }
 648
 649 static void viacam_vb2_stop_streaming(struct vb2_queue *vq)
 650 {
 651         struct via_camera *cam = vb2_get_drv_priv(vq);
 652         struct via_buffer *buf, *tmp;
 653
 654         cpu_latency_qos_remove_request(&cam->qos_request);
 655         viacam_stop_engine(cam);
 656
 657         list_for_each_entry_safe(buf, tmp, &cam->buffer_queue, queue) {
 658                 list_del(&buf->queue);
 659                 vb2_buffer_done(&buf->vbuf.vb2_buf, VB2_BUF_STATE_ERROR);
 660         }
 661 }
 662
 663 static const struct vb2_ops viacam_vb2_ops = {
 664         .queue_setup            = viacam_vb2_queue_setup,
 665         .buf_queue              = viacam_vb2_queue,
 666         .buf_prepare            = viacam_vb2_prepare,
 667         .start_streaming        = viacam_vb2_start_streaming,
 668         .stop_streaming         = viacam_vb2_stop_streaming,
 669         .wait_prepare           = vb2_ops_wait_prepare,
 670         .wait_finish            = vb2_ops_wait_finish,
 671 };
 672
 673 /* --------------------------------------------------------------------------*/
 674 /* File operations */
 675
 676 static int viacam_open(struct file *filp)
 677 {
 678         struct via_camera *cam = video_drvdata(filp);
 679         int ret;
 680
 681         /*
 682          * Note the new user.  If this is the first one, we'll also
 683          * need to power up the sensor.
 684          */
 685         mutex_lock(&cam->lock);
 686         ret = v4l2_fh_open(filp);
 687         if (ret)
 688                 goto out;
 689         if (v4l2_fh_is_singular_file(filp)) {
 690                 ret = viafb_request_dma();
 691
 692                 if (ret) {
 693                         v4l2_fh_release(filp);
 694                         goto out;
 695                 }
 696                 via_sensor_power_up(cam);
 697                 set_bit(CF_CONFIG_NEEDED, &cam->flags);
 698         }
 699 out:
 700         mutex_unlock(&cam->lock);
 701         return ret;
 702 }
 703
 704 static int viacam_release(struct file *filp)
 705 {
 706         struct via_camera *cam = video_drvdata(filp);
 707         bool last_open;
 708
 709         mutex_lock(&cam->lock);
 710         last_open = v4l2_fh_is_singular_file(filp);
 711         _vb2_fop_release(filp, NULL);
 712         /*
 713          * Last one out needs to turn out the lights.
 714          */
 715         if (last_open) {
 716                 via_sensor_power_down(cam);
 717                 viafb_release_dma();
 718         }
 719         mutex_unlock(&cam->lock);
 720         return 0;
 721 }
 722
 723 static const struct v4l2_file_operations viacam_fops = {
 724         .owner          = THIS_MODULE,
 725         .open           = viacam_open,
 726         .release        = viacam_release,
 727         .read           = vb2_fop_read,
 728         .poll           = vb2_fop_poll,
 729         .mmap           = vb2_fop_mmap,
 730         .unlocked_ioctl = video_ioctl2,
 731 };
 732
 733 /*----------------------------------------------------------------------------*/
 734 /*
 735  * The long list of v4l2 ioctl ops
 736  */
 737
 738 /*
 739  * Only one input.
 740  */
 741 static int viacam_enum_input(struct file *filp, void *priv,
 742                 struct v4l2_input *input)
 743 {
 744         if (input->index != 0)
 745                 return -EINVAL;
 746
 747         input->type = V4L2_INPUT_TYPE_CAMERA;
 748         strscpy(input->name, "Camera", sizeof(input->name));
 749         return 0;
 750 }
 751
 752 static int viacam_g_input(struct file *filp, void *priv, unsigned int *i)
 753 {
 754         *i = 0;
 755         return 0;
 756 }
 757
 758 static int viacam_s_input(struct file *filp, void *priv, unsigned int i)
 759 {
 760         if (i != 0)
 761                 return -EINVAL;
 762         return 0;
 763 }
 764
 765 /*
 766  * Video format stuff.  Here is our default format until
 767  * user space messes with things.
 768  */
 769 static const struct v4l2_pix_format viacam_def_pix_format = {
 770         .width          = VGA_WIDTH,
 771         .height         = VGA_HEIGHT,
 772         .pixelformat    = V4L2_PIX_FMT_YUYV,
 773         .field          = V4L2_FIELD_NONE,
 774         .bytesperline   = VGA_WIDTH * 2,
 775         .sizeimage      = VGA_WIDTH * VGA_HEIGHT * 2,
 776         .colorspace     = V4L2_COLORSPACE_SRGB,
 777 };
 778
 779 static const u32 via_def_mbus_code = MEDIA_BUS_FMT_YUYV8_2X8;
 780
 781 static int viacam_enum_fmt_vid_cap(struct file *filp, void *priv,
 782                 struct v4l2_fmtdesc *fmt)
 783 {
 784         if (fmt->index >= N_VIA_FMTS)
 785                 return -EINVAL;
 786         fmt->pixelformat = via_formats[fmt->index].pixelformat;
 787         return 0;
 788 }
 789
 790 /*
 791  * Figure out proper image dimensions, but always force the
 792  * sensor to VGA.
 793  */
 794 static void viacam_fmt_pre(struct v4l2_pix_format *userfmt,
 795                 struct v4l2_pix_format *sensorfmt)
 796 {
 797         *sensorfmt = *userfmt;
 798         if (userfmt->width < QCIF_WIDTH || userfmt->height < QCIF_HEIGHT) {
 799                 userfmt->width = QCIF_WIDTH;
 800                 userfmt->height = QCIF_HEIGHT;
 801         }
 802         if (userfmt->width > VGA_WIDTH || userfmt->height > VGA_HEIGHT) {
 803                 userfmt->width = VGA_WIDTH;
 804                 userfmt->height = VGA_HEIGHT;
 805         }
 806         sensorfmt->width = VGA_WIDTH;
 807         sensorfmt->height = VGA_HEIGHT;
 808 }
 809
 810 static void viacam_fmt_post(struct v4l2_pix_format *userfmt,
 811                 struct v4l2_pix_format *sensorfmt)
 812 {
 813         struct via_format *f = via_find_format(userfmt->pixelformat);
 814
 815         sensorfmt->bytesperline = sensorfmt->width * f->bpp;
 816         sensorfmt->sizeimage = sensorfmt->height * sensorfmt->bytesperline;
 817         userfmt->pixelformat = sensorfmt->pixelformat;
 818         userfmt->field = sensorfmt->field;
 819         userfmt->bytesperline = 2 * userfmt->width;
 820         userfmt->sizeimage = userfmt->bytesperline * userfmt->height;
 821         userfmt->colorspace = sensorfmt->colorspace;
 822         userfmt->ycbcr_enc = sensorfmt->ycbcr_enc;
 823         userfmt->quantization = sensorfmt->quantization;
 824         userfmt->xfer_func = sensorfmt->xfer_func;
 825 }
 826
 827
 828 /*
 829  * The real work of figuring out a workable format.
 830  */
 831 static int viacam_do_try_fmt(struct via_camera *cam,
 832                 struct v4l2_pix_format *upix, struct v4l2_pix_format *spix)
 833 {
 834         int ret;
 835         struct v4l2_subdev_pad_config pad_cfg;
 836         struct v4l2_subdev_state pad_state = {
 837                 .pads = &pad_cfg,
 838         };
 839         struct v4l2_subdev_format format = {
 840                 .which = V4L2_SUBDEV_FORMAT_TRY,
 841         };
 842         struct via_format *f = via_find_format(upix->pixelformat);
 843
 844         upix->pixelformat = f->pixelformat;
 845         viacam_fmt_pre(upix, spix);
 846         v4l2_fill_mbus_format(&format.format, spix, f->mbus_code);
 847         ret = sensor_call(cam, pad, set_fmt, &pad_state, &format);
 848         v4l2_fill_pix_format(spix, &format.format);
 849         viacam_fmt_post(upix, spix);
 850         return ret;
 851 }
 852
 853
 854
 855 static int viacam_try_fmt_vid_cap(struct file *filp, void *priv,
 856                 struct v4l2_format *fmt)
 857 {
 858         struct via_camera *cam = video_drvdata(filp);
 859         struct v4l2_format sfmt;
 860
 861         return viacam_do_try_fmt(cam, &fmt->fmt.pix, &sfmt.fmt.pix);
 862 }
 863
 864
 865 static int viacam_g_fmt_vid_cap(struct file *filp, void *priv,
 866                 struct v4l2_format *fmt)
 867 {
 868         struct via_camera *cam = video_drvdata(filp);
 869
 870         fmt->fmt.pix = cam->user_format;
 871         return 0;
 872 }
 873
 874 static int viacam_s_fmt_vid_cap(struct file *filp, void *priv,
 875                 struct v4l2_format *fmt)
 876 {
 877         struct via_camera *cam = video_drvdata(filp);
 878         int ret;
 879         struct v4l2_format sfmt;
 880         struct via_format *f = via_find_format(fmt->fmt.pix.pixelformat);
 881
 882         /*
 883          * Camera must be idle or we can't mess with the
 884          * video setup.
 885          */
 886         if (cam->opstate != S_IDLE)
 887                 return -EBUSY;
 888         /*
 889          * Let the sensor code look over and tweak the
 890          * requested formatting.
 891          */
 892         ret = viacam_do_try_fmt(cam, &fmt->fmt.pix, &sfmt.fmt.pix);
 893         if (ret)
 894                 return ret;
 895         /*
 896          * OK, let's commit to the new format.
 897          */
 898         cam->user_format = fmt->fmt.pix;
 899         cam->sensor_format = sfmt.fmt.pix;
 900         cam->mbus_code = f->mbus_code;
 901         ret = viacam_configure_sensor(cam);
 902         if (!ret)
 903                 ret = viacam_config_controller(cam);
 904         return ret;
 905 }
 906
 907 static int viacam_querycap(struct file *filp, void *priv,
 908                 struct v4l2_capability *cap)
 909 {
 910         strscpy(cap->driver, "via-camera", sizeof(cap->driver));
 911         strscpy(cap->card, "via-camera", sizeof(cap->card));
 912         strscpy(cap->bus_info, "platform:via-camera", sizeof(cap->bus_info));
 913         return 0;
 914 }
 915
 916 /* G/S_PARM */
 917
 918 static int viacam_g_parm(struct file *filp, void *priv,
 919                 struct v4l2_streamparm *parm)
 920 {
 921         struct via_camera *cam = video_drvdata(filp);
 922
 923         return v4l2_g_parm_cap(video_devdata(filp), cam->sensor, parm);
 924 }
 925
 926 static int viacam_s_parm(struct file *filp, void *priv,
 927                 struct v4l2_streamparm *parm)
 928 {
 929         struct via_camera *cam = video_drvdata(filp);
 930
 931         return v4l2_s_parm_cap(video_devdata(filp), cam->sensor, parm);
 932 }
 933
 934 static int viacam_enum_framesizes(struct file *filp, void *priv,
 935                 struct v4l2_frmsizeenum *sizes)
 936 {
 937         unsigned int i;
 938
 939         if (sizes->index != 0)
 940                 return -EINVAL;
 941         for (i = 0; i < N_VIA_FMTS; i++)
 942                 if (sizes->pixel_format == via_formats[i].pixelformat)
 943                         break;
 944         if (i >= N_VIA_FMTS)
 945                 return -EINVAL;
 946         sizes->type = V4L2_FRMSIZE_TYPE_CONTINUOUS;
 947         sizes->stepwise.min_width = QCIF_WIDTH;
 948         sizes->stepwise.min_height = QCIF_HEIGHT;
 949         sizes->stepwise.max_width = VGA_WIDTH;
 950         sizes->stepwise.max_height = VGA_HEIGHT;
 951         sizes->stepwise.step_width = sizes->stepwise.step_height = 1;
 952         return 0;
 953 }
 954
 955 static int viacam_enum_frameintervals(struct file *filp, void *priv,
 956                 struct v4l2_frmivalenum *interval)
 957 {
 958         struct via_camera *cam = video_drvdata(filp);
 959         struct v4l2_subdev_frame_interval_enum fie = {
 960                 .index = interval->index,
 961                 .code = cam->mbus_code,
 962                 .width = cam->sensor_format.width,
 963                 .height = cam->sensor_format.height,
 964                 .which = V4L2_SUBDEV_FORMAT_ACTIVE,
 965         };
 966         unsigned int i;
 967         int ret;
 968
 969         for (i = 0; i < N_VIA_FMTS; i++)
 970                 if (interval->pixel_format == via_formats[i].pixelformat)
 971                         break;
 972         if (i >= N_VIA_FMTS)
 973                 return -EINVAL;
 974         if (interval->width < QCIF_WIDTH || interval->width > VGA_WIDTH ||
 975             interval->height < QCIF_HEIGHT || interval->height > VGA_HEIGHT)
 976                 return -EINVAL;
 977         ret = sensor_call(cam, pad, enum_frame_interval, NULL, &fie);
 978         if (ret)
 979                 return ret;
 980         interval->type = V4L2_FRMIVAL_TYPE_DISCRETE;
 981         interval->discrete = fie.interval;
 982         return 0;
 983 }
 984
 985 static const struct v4l2_ioctl_ops viacam_ioctl_ops = {
 986         .vidioc_enum_input      = viacam_enum_input,
 987         .vidioc_g_input         = viacam_g_input,
 988         .vidioc_s_input         = viacam_s_input,
 989         .vidioc_enum_fmt_vid_cap = viacam_enum_fmt_vid_cap,
 990         .vidioc_try_fmt_vid_cap = viacam_try_fmt_vid_cap,
 991         .vidioc_g_fmt_vid_cap   = viacam_g_fmt_vid_cap,
 992         .vidioc_s_fmt_vid_cap   = viacam_s_fmt_vid_cap,
 993         .vidioc_querycap        = viacam_querycap,
 994         .vidioc_reqbufs         = vb2_ioctl_reqbufs,
 995         .vidioc_create_bufs     = vb2_ioctl_create_bufs,
 996         .vidioc_querybuf        = vb2_ioctl_querybuf,
 997         .vidioc_prepare_buf     = vb2_ioctl_prepare_buf,
 998         .vidioc_qbuf            = vb2_ioctl_qbuf,
 999         .vidioc_dqbuf           = vb2_ioctl_dqbuf,
1000         .vidioc_expbuf          = vb2_ioctl_expbuf,
1001         .vidioc_streamon        = vb2_ioctl_streamon,
1002         .vidioc_streamoff       = vb2_ioctl_streamoff,
1003         .vidioc_g_parm          = viacam_g_parm,
1004         .vidioc_s_parm          = viacam_s_parm,
1005         .vidioc_enum_framesizes = viacam_enum_framesizes,
1006         .vidioc_enum_frameintervals = viacam_enum_frameintervals,
1007         .vidioc_subscribe_event         = v4l2_ctrl_subscribe_event,
1008         .vidioc_unsubscribe_event       = v4l2_event_unsubscribe,
1009 };
1010
1011 /*----------------------------------------------------------------------------*/
1012
1013 /*
1014  * Power management.
1015  */
1016 #ifdef CONFIG_PM
1017
1018 static int viacam_suspend(void *priv)
1019 {
1020         struct via_camera *cam = priv;
1021         enum viacam_opstate state = cam->opstate;
1022
1023         if (cam->opstate != S_IDLE) {
1024                 viacam_stop_engine(cam);
1025                 cam->opstate = state; /* So resume restarts */
1026         }
1027
1028         return 0;
1029 }
1030
1031 static int viacam_resume(void *priv)
1032 {
1033         struct via_camera *cam = priv;
1034         int ret = 0;
1035
1036         /*
1037          * Get back to a reasonable operating state.
1038          */
1039         via_write_reg_mask(VIASR, 0x78, 0, 0x80);
1040         via_write_reg_mask(VIASR, 0x1e, 0xc0, 0xc0);
1041         viacam_int_disable(cam);
1042         set_bit(CF_CONFIG_NEEDED, &cam->flags);
1043         /*
1044          * Make sure the sensor's power state is correct
1045          */
1046         if (!list_empty(&cam->vdev.fh_list))
1047                 via_sensor_power_up(cam);
1048         else
1049                 via_sensor_power_down(cam);
1050         /*
1051          * If it was operating, try to restart it.
1052          */
1053         if (cam->opstate != S_IDLE) {
1054                 mutex_lock(&cam->lock);
1055                 ret = viacam_configure_sensor(cam);
1056                 if (!ret)
1057                         ret = viacam_config_controller(cam);
1058                 mutex_unlock(&cam->lock);
1059                 if (!ret)
1060                         viacam_start_engine(cam);
1061         }
1062
1063         return ret;
1064 }
1065
1066 static struct viafb_pm_hooks viacam_pm_hooks = {
1067         .suspend = viacam_suspend,
1068         .resume = viacam_resume
1069 };
1070
1071 #endif /* CONFIG_PM */
1072
1073 /*
1074  * Setup stuff.
1075  */
1076
1077 static const struct video_device viacam_v4l_template = {
1078         .name           = "via-camera",
1079         .minor          = -1,
1080         .fops           = &viacam_fops,
1081         .ioctl_ops      = &viacam_ioctl_ops,
1082         .release        = video_device_release_empty, /* Check this */
1083         .device_caps    = V4L2_CAP_VIDEO_CAPTURE | V4L2_CAP_READWRITE |
1084                           V4L2_CAP_STREAMING,
1085 };
1086
1087 /*
1088  * The OLPC folks put the serial port on the same pin as
1089  * the camera.  They also get grumpy if we break the
1090  * serial port and keep them from using it.  So we have
1091  * to check the serial enable bit and not step on it.
1092  */
1093 #define VIACAM_SERIAL_DEVFN 0x88
1094 #define VIACAM_SERIAL_CREG 0x46
1095 #define VIACAM_SERIAL_BIT 0x40
1096
1097 static bool viacam_serial_is_enabled(void)
1098 {
1099         struct pci_bus *pbus = pci_find_bus(0, 0);
1100         u8 cbyte;
1101
1102         if (!pbus)
1103                 return false;
1104         pci_bus_read_config_byte(pbus, VIACAM_SERIAL_DEVFN,
1105                         VIACAM_SERIAL_CREG, &cbyte);
1106         if ((cbyte & VIACAM_SERIAL_BIT) == 0)
1107                 return false; /* Not enabled */
1108         if (!override_serial) {
1109                 printk(KERN_NOTICE "Via camera: serial port is enabled, " \
1110                                 "refusing to load.\n");
1111                 printk(KERN_NOTICE "Specify override_serial=1 to force " \
1112                                 "module loading.\n");
1113                 return true;
1114         }
1115         printk(KERN_NOTICE "Via camera: overriding serial port\n");
1116         pci_bus_write_config_byte(pbus, VIACAM_SERIAL_DEVFN,
1117                         VIACAM_SERIAL_CREG, cbyte & ~VIACAM_SERIAL_BIT);
1118         return false;
1119 }
1120
1121 static struct ov7670_config sensor_cfg = {
1122         /* The XO-1.5 (only known user) clocks the camera at 90MHz. */
1123         .clock_speed = 90,
1124 };
1125
1126 static int viacam_probe(struct platform_device *pdev)
1127 {
1128         int ret;
1129         struct i2c_adapter *sensor_adapter;
1130         struct viafb_dev *viadev = pdev->dev.platform_data;
1131         struct vb2_queue *vq;
1132         struct i2c_board_info ov7670_info = {
1133                 .type = "ov7670",
1134                 .addr = 0x42 >> 1,
1135                 .platform_data = &sensor_cfg,
1136         };
1137
1138         /*
1139          * Note that there are actually two capture channels on
1140          * the device.  We only deal with one for now.  That
1141          * is encoded here; nothing else assumes it's dealing with
1142          * a unique capture device.
1143          */
1144         struct via_camera *cam;
1145
1146         /*
1147          * Ensure that frame buffer memory has been set aside for
1148          * this purpose.  As an arbitrary limit, refuse to work
1149          * with less than two frames of VGA 16-bit data.
1150          *
1151          * If we ever support the second port, we'll need to set
1152          * aside more memory.
1153          */
1154         if (viadev->camera_fbmem_size < (VGA_HEIGHT*VGA_WIDTH*4)) {
1155                 printk(KERN_ERR "viacam: insufficient FB memory reserved\n");
1156                 return -ENOMEM;
1157         }
1158         if (viadev->engine_mmio == NULL) {
1159                 printk(KERN_ERR "viacam: No I/O memory, so no pictures\n");
1160                 return -ENOMEM;
1161         }
1162
1163         if (machine_is_olpc() && viacam_serial_is_enabled())
1164                 return -EBUSY;
1165
1166         /*
1167          * Basic structure initialization.
1168          */
1169         cam = kzalloc (sizeof(struct via_camera), GFP_KERNEL);
1170         if (cam == NULL)
1171                 return -ENOMEM;
1172         via_cam_info = cam;
1173         cam->platdev = pdev;
1174         cam->viadev = viadev;
1175         cam->opstate = S_IDLE;
1176         cam->user_format = cam->sensor_format = viacam_def_pix_format;
1177         mutex_init(&cam->lock);
1178         INIT_LIST_HEAD(&cam->buffer_queue);
1179         cam->mmio = viadev->engine_mmio;
1180         cam->fbmem = viadev->fbmem;
1181         cam->fb_offset = viadev->camera_fbmem_offset;
1182         cam->flags = 1 << CF_CONFIG_NEEDED;
1183         cam->mbus_code = via_def_mbus_code;
1184         /*
1185          * Tell V4L that we exist.
1186          */
1187         ret = v4l2_device_register(&pdev->dev, &cam->v4l2_dev);
1188         if (ret) {
1189                 dev_err(&pdev->dev, "Unable to register v4l2 device\n");
1190                 goto out_free;
1191         }
1192         ret = v4l2_ctrl_handler_init(&cam->ctrl_handler, 10);
1193         if (ret)
1194                 goto out_unregister;
1195         cam->v4l2_dev.ctrl_handler = &cam->ctrl_handler;
1196         /*
1197          * Convince the system that we can do DMA.
1198          */
1199         pdev->dev.dma_mask = &viadev->pdev->dma_mask;
1200         ret = dma_set_mask(&pdev->dev, 0xffffffff);
1201         if (ret)
1202                 goto out_ctrl_hdl_free;
1203         /*
1204          * Fire up the capture port.  The write to 0x78 looks purely
1205          * OLPCish; any system will need to tweak 0x1e.
1206          */
1207         via_write_reg_mask(VIASR, 0x78, 0, 0x80);
1208         via_write_reg_mask(VIASR, 0x1e, 0xc0, 0xc0);
1209         /*
1210          * Get the sensor powered up.
1211          */
1212         ret = via_sensor_power_setup(cam);
1213         if (ret)
1214                 goto out_ctrl_hdl_free;
1215         via_sensor_power_up(cam);
1216
1217         /*
1218          * See if we can't find it on the bus.  The VIA_PORT_31 assumption
1219          * is OLPC-specific.  0x42 assumption is ov7670-specific.
1220          */
1221         sensor_adapter = viafb_find_i2c_adapter(VIA_PORT_31);
1222         cam->sensor = v4l2_i2c_new_subdev_board(&cam->v4l2_dev, sensor_adapter,
1223                         &ov7670_info, NULL);
1224         if (cam->sensor == NULL) {
1225                 dev_err(&pdev->dev, "Unable to find the sensor!\n");
1226                 ret = -ENODEV;
1227                 goto out_power_down;
1228         }
1229         /*
1230          * Get the IRQ.
1231          */
1232         viacam_int_disable(cam);
1233         ret = request_threaded_irq(viadev->pdev->irq, viacam_quick_irq,
1234                         viacam_irq, IRQF_SHARED, "via-camera", cam);
1235         if (ret)
1236                 goto out_power_down;
1237
1238         vq = &cam->vq;
1239         vq->type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
1240         vq->io_modes = VB2_MMAP | VB2_USERPTR | VB2_DMABUF | VB2_READ;
1241         vq->drv_priv = cam;
1242         vq->timestamp_flags = V4L2_BUF_FLAG_TIMESTAMP_MONOTONIC;
1243         vq->buf_struct_size = sizeof(struct via_buffer);
1244         vq->dev = cam->v4l2_dev.dev;
1245
1246         vq->ops = &viacam_vb2_ops;
1247         vq->mem_ops = &vb2_dma_sg_memops;
1248         vq->lock = &cam->lock;
1249
1250         ret = vb2_queue_init(vq);
1251         /*
1252          * Tell V4l2 that we exist.
1253          */
1254         cam->vdev = viacam_v4l_template;
1255         cam->vdev.v4l2_dev = &cam->v4l2_dev;
1256         cam->vdev.lock = &cam->lock;
1257         cam->vdev.queue = vq;
1258         video_set_drvdata(&cam->vdev, cam);
1259         ret = video_register_device(&cam->vdev, VFL_TYPE_VIDEO, -1);
1260         if (ret)
1261                 goto out_irq;
1262
1263 #ifdef CONFIG_PM
1264         /*
1265          * Hook into PM events
1266          */
1267         viacam_pm_hooks.private = cam;
1268         viafb_pm_register(&viacam_pm_hooks);
1269 #endif
1270
1271         /* Power the sensor down until somebody opens the device */
1272         via_sensor_power_down(cam);
1273         return 0;
1274
1275 out_irq:
1276         free_irq(viadev->pdev->irq, cam);
1277 out_power_down:
1278         via_sensor_power_release(cam);
1279 out_ctrl_hdl_free:
1280         v4l2_ctrl_handler_free(&cam->ctrl_handler);
1281 out_unregister:
1282         v4l2_device_unregister(&cam->v4l2_dev);
1283 out_free:
1284         kfree(cam);
1285         return ret;
1286 }
1287
1288 static void viacam_remove(struct platform_device *pdev)
1289 {
1290         struct via_camera *cam = via_cam_info;
1291         struct viafb_dev *viadev = pdev->dev.platform_data;
1292
1293         video_unregister_device(&cam->vdev);
1294         v4l2_device_unregister(&cam->v4l2_dev);
1295 #ifdef CONFIG_PM
1296         viafb_pm_unregister(&viacam_pm_hooks);
1297 #endif
1298         free_irq(viadev->pdev->irq, cam);
1299         via_sensor_power_release(cam);
1300         v4l2_ctrl_handler_free(&cam->ctrl_handler);
1301         kfree(cam);
1302         via_cam_info = NULL;
1303 }
1304
1305 static struct platform_driver viacam_driver = {
1306         .driver = {
1307                 .name = "viafb-camera",
1308         },
1309         .probe = viacam_probe,
1310         .remove_new = viacam_remove,
1311 };
1312
1313 module_platform_driver(viacam_driver);