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