xref: /linux/drivers/media/platform/via/via-camera.c (revision cdd5b5a9761fd66d17586e4f4ba6588c70e640ea)
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