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
via_find_format(u32 pixelformat)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 */
via_sensor_power_setup(struct via_camera * cam)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 */
via_sensor_power_up(struct via_camera * cam)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
via_sensor_power_down(struct via_camera * cam)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
via_sensor_power_release(struct via_camera * cam)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 */
viacam_set_flip(struct via_camera * cam)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 */
viacam_configure_sensor(struct via_camera * cam)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 */
viacam_write_reg(struct via_camera * cam,int reg,int value)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
viacam_read_reg(struct via_camera * cam,int reg)277 static inline int viacam_read_reg(struct via_camera *cam, int reg)
278 {
279 return ioread32(cam->mmio + reg);
280 }
281
viacam_write_reg_mask(struct via_camera * cam,int reg,int value,int mask)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
viacam_quick_irq(int irq,void * data)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 */
viacam_next_buffer(struct via_camera * cam)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 */
viacam_irq(int irq,void * data)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 */
viacam_int_enable(struct via_camera * cam)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
viacam_int_disable(struct via_camera * cam)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 */
viacam_ctlr_cbufs(struct via_camera * cam)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 */
viacam_set_scale(struct via_camera * cam)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 */
viacam_ctlr_image(struct via_camera * cam)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
viacam_config_controller(struct via_camera * cam)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 */
viacam_start_engine(struct via_camera * cam)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
viacam_stop_engine(struct via_camera * cam)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
vb2_to_via_buffer(struct vb2_buffer * vb)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
viacam_vb2_queue(struct vb2_buffer * vb)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
viacam_vb2_prepare(struct vb2_buffer * vb)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
viacam_vb2_queue_setup(struct vb2_queue * vq,unsigned int * nbufs,unsigned int * num_planes,unsigned int sizes[],struct device * alloc_devs[])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
viacam_vb2_start_streaming(struct vb2_queue * vq,unsigned int count)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
viacam_vb2_stop_streaming(struct vb2_queue * vq)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
viacam_open(struct file * filp)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
viacam_release(struct file * filp)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 */
viacam_enum_input(struct file * filp,void * priv,struct v4l2_input * input)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
viacam_g_input(struct file * filp,void * priv,unsigned int * i)752 static int viacam_g_input(struct file *filp, void *priv, unsigned int *i)
753 {
754 *i = 0;
755 return 0;
756 }
757
viacam_s_input(struct file * filp,void * priv,unsigned int i)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
viacam_enum_fmt_vid_cap(struct file * filp,void * priv,struct v4l2_fmtdesc * fmt)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 */
viacam_fmt_pre(struct v4l2_pix_format * userfmt,struct v4l2_pix_format * sensorfmt)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
viacam_fmt_post(struct v4l2_pix_format * userfmt,struct v4l2_pix_format * sensorfmt)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 */
viacam_do_try_fmt(struct via_camera * cam,struct v4l2_pix_format * upix,struct v4l2_pix_format * spix)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
viacam_try_fmt_vid_cap(struct file * filp,void * priv,struct v4l2_format * fmt)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
viacam_g_fmt_vid_cap(struct file * filp,void * priv,struct v4l2_format * fmt)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
viacam_s_fmt_vid_cap(struct file * filp,void * priv,struct v4l2_format * fmt)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
viacam_querycap(struct file * filp,void * priv,struct v4l2_capability * cap)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
viacam_g_parm(struct file * filp,void * priv,struct v4l2_streamparm * parm)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
viacam_s_parm(struct file * filp,void * priv,struct v4l2_streamparm * parm)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
viacam_enum_framesizes(struct file * filp,void * priv,struct v4l2_frmsizeenum * sizes)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
viacam_enum_frameintervals(struct file * filp,void * priv,struct v4l2_frmivalenum * interval)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
viacam_suspend(void * priv)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
viacam_resume(void * priv)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
viacam_serial_is_enabled(void)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
viacam_probe(struct platform_device * pdev)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
viacam_remove(struct platform_device * pdev)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);
1314