via-camera.c

   1// SPDX-License-Identifier: GPL-2.0-only
   2/*
   3 * Driver for the VIA Chrome integrated camera controller.
   4 *
   5 * Copyright 2009,2010 Jonathan Corbet <corbet@lwn.net>
   6 *
   7 * This work was supported by the One Laptop Per Child project
   8 */
   9#include <linux/kernel.h>
  10#include <linux/module.h>
  11#include <linux/device.h>
  12#include <linux/list.h>
  13#include <linux/pci.h>
  14#include <linux/gpio/consumer.h>
  15#include <linux/interrupt.h>
  16#include <linux/platform_device.h>
  17#include <linux/videodev2.h>
  18#include <media/v4l2-device.h>
  19#include <media/v4l2-ioctl.h>
  20#include <media/v4l2-ctrls.h>
  21#include <media/v4l2-event.h>
  22#include <media/v4l2-image-sizes.h>
  23#include <media/i2c/ov7670.h>
  24#include <media/videobuf2-dma-sg.h>
  25#include <linux/delay.h>
  26#include <linux/dma-mapping.h>
  27#include <linux/pm_qos.h>
  28#include <linux/via-core.h>
  29#include <linux/via_i2c.h>
  30
  31#ifdef CONFIG_X86
  32#include <asm/olpc.h>
  33#else
  34#define machine_is_olpc(x) 0
  35#endif
  36
  37#include "via-camera.h"
  38
  39MODULE_ALIAS("platform:viafb-camera");
  40MODULE_AUTHOR("Jonathan Corbet <corbet@lwn.net>");
  41MODULE_DESCRIPTION("VIA framebuffer-based camera controller driver");
  42MODULE_LICENSE("GPL");
  43
  44static bool flip_image;
  45module_param(flip_image, bool, 0444);
  46MODULE_PARM_DESC(flip_image,
  47		"If set, the sensor will be instructed to flip the image vertically.");
  48
  49static bool override_serial;
  50module_param(override_serial, bool, 0444);
  51MODULE_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 */
  57enum viacam_opstate { S_IDLE = 0, S_RUNNING = 1 };
  58
  59struct 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 */
 105struct 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 */
 115static 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 */
 146static 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
 163static 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 */
 180static 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 */
 201static 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
 210static 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
 217static 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 */
 228static 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 */
 242static 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 */
 271static inline void viacam_write_reg(struct via_camera *cam,
 272		int reg, int value)
 273{
 274	iowrite32(value, cam->mmio + reg);
 275}
 276
 277static inline int viacam_read_reg(struct via_camera *cam, int reg)
 278{
 279	return ioread32(cam->mmio + reg);
 280}
 281
 282static 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
 295static 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 */
 319static 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 */
 331static 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);
 363done:
 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 */
 376static 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
 383static 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 */
 397static 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 */
 447static 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 */
 469static 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
 520static 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 */
 537static 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
 548static 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
 562static 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
 569static 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
 577static 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
 594static 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
 610static 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;
 641out:
 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
 649static 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
 663static 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};
 670
 671/* --------------------------------------------------------------------------*/
 672/* File operations */
 673
 674static int viacam_open(struct file *filp)
 675{
 676	struct via_camera *cam = video_drvdata(filp);
 677	int ret;
 678
 679	/*
 680	 * Note the new user.  If this is the first one, we'll also
 681	 * need to power up the sensor.
 682	 */
 683	mutex_lock(&cam->lock);
 684	ret = v4l2_fh_open(filp);
 685	if (ret)
 686		goto out;
 687	if (v4l2_fh_is_singular_file(filp)) {
 688		ret = viafb_request_dma();
 689
 690		if (ret) {
 691			v4l2_fh_release(filp);
 692			goto out;
 693		}
 694		via_sensor_power_up(cam);
 695		set_bit(CF_CONFIG_NEEDED, &cam->flags);
 696	}
 697out:
 698	mutex_unlock(&cam->lock);
 699	return ret;
 700}
 701
 702static int viacam_release(struct file *filp)
 703{
 704	struct via_camera *cam = video_drvdata(filp);
 705	bool last_open;
 706
 707	mutex_lock(&cam->lock);
 708	last_open = v4l2_fh_is_singular_file(filp);
 709	_vb2_fop_release(filp, NULL);
 710	/*
 711	 * Last one out needs to turn out the lights.
 712	 */
 713	if (last_open) {
 714		via_sensor_power_down(cam);
 715		viafb_release_dma();
 716	}
 717	mutex_unlock(&cam->lock);
 718	return 0;
 719}
 720
 721static const struct v4l2_file_operations viacam_fops = {
 722	.owner		= THIS_MODULE,
 723	.open		= viacam_open,
 724	.release	= viacam_release,
 725	.read		= vb2_fop_read,
 726	.poll		= vb2_fop_poll,
 727	.mmap		= vb2_fop_mmap,
 728	.unlocked_ioctl = video_ioctl2,
 729};
 730
 731/*----------------------------------------------------------------------------*/
 732/*
 733 * The long list of v4l2 ioctl ops
 734 */
 735
 736/*
 737 * Only one input.
 738 */
 739static int viacam_enum_input(struct file *filp, void *priv,
 740		struct v4l2_input *input)
 741{
 742	if (input->index != 0)
 743		return -EINVAL;
 744
 745	input->type = V4L2_INPUT_TYPE_CAMERA;
 746	strscpy(input->name, "Camera", sizeof(input->name));
 747	return 0;
 748}
 749
 750static int viacam_g_input(struct file *filp, void *priv, unsigned int *i)
 751{
 752	*i = 0;
 753	return 0;
 754}
 755
 756static int viacam_s_input(struct file *filp, void *priv, unsigned int i)
 757{
 758	if (i != 0)
 759		return -EINVAL;
 760	return 0;
 761}
 762
 763/*
 764 * Video format stuff.	Here is our default format until
 765 * user space messes with things.
 766 */
 767static const struct v4l2_pix_format viacam_def_pix_format = {
 768	.width		= VGA_WIDTH,
 769	.height		= VGA_HEIGHT,
 770	.pixelformat	= V4L2_PIX_FMT_YUYV,
 771	.field		= V4L2_FIELD_NONE,
 772	.bytesperline	= VGA_WIDTH * 2,
 773	.sizeimage	= VGA_WIDTH * VGA_HEIGHT * 2,
 774	.colorspace	= V4L2_COLORSPACE_SRGB,
 775};
 776
 777static const u32 via_def_mbus_code = MEDIA_BUS_FMT_YUYV8_2X8;
 778
 779static int viacam_enum_fmt_vid_cap(struct file *filp, void *priv,
 780		struct v4l2_fmtdesc *fmt)
 781{
 782	if (fmt->index >= N_VIA_FMTS)
 783		return -EINVAL;
 784	fmt->pixelformat = via_formats[fmt->index].pixelformat;
 785	return 0;
 786}
 787
 788/*
 789 * Figure out proper image dimensions, but always force the
 790 * sensor to VGA.
 791 */
 792static void viacam_fmt_pre(struct v4l2_pix_format *userfmt,
 793		struct v4l2_pix_format *sensorfmt)
 794{
 795	*sensorfmt = *userfmt;
 796	if (userfmt->width < QCIF_WIDTH || userfmt->height < QCIF_HEIGHT) {
 797		userfmt->width = QCIF_WIDTH;
 798		userfmt->height = QCIF_HEIGHT;
 799	}
 800	if (userfmt->width > VGA_WIDTH || userfmt->height > VGA_HEIGHT) {
 801		userfmt->width = VGA_WIDTH;
 802		userfmt->height = VGA_HEIGHT;
 803	}
 804	sensorfmt->width = VGA_WIDTH;
 805	sensorfmt->height = VGA_HEIGHT;
 806}
 807
 808static void viacam_fmt_post(struct v4l2_pix_format *userfmt,
 809		struct v4l2_pix_format *sensorfmt)
 810{
 811	struct via_format *f = via_find_format(userfmt->pixelformat);
 812
 813	sensorfmt->bytesperline = sensorfmt->width * f->bpp;
 814	sensorfmt->sizeimage = sensorfmt->height * sensorfmt->bytesperline;
 815	userfmt->pixelformat = sensorfmt->pixelformat;
 816	userfmt->field = sensorfmt->field;
 817	userfmt->bytesperline = 2 * userfmt->width;
 818	userfmt->sizeimage = userfmt->bytesperline * userfmt->height;
 819	userfmt->colorspace = sensorfmt->colorspace;
 820	userfmt->ycbcr_enc = sensorfmt->ycbcr_enc;
 821	userfmt->quantization = sensorfmt->quantization;
 822	userfmt->xfer_func = sensorfmt->xfer_func;
 823}
 824
 825
 826/*
 827 * The real work of figuring out a workable format.
 828 */
 829static int viacam_do_try_fmt(struct via_camera *cam,
 830		struct v4l2_pix_format *upix, struct v4l2_pix_format *spix)
 831{
 832	int ret;
 833	struct v4l2_subdev_pad_config pad_cfg;
 834	struct v4l2_subdev_state pad_state = {
 835		.pads = &pad_cfg,
 836	};
 837	struct v4l2_subdev_format format = {
 838		.which = V4L2_SUBDEV_FORMAT_TRY,
 839	};
 840	struct via_format *f = via_find_format(upix->pixelformat);
 841
 842	upix->pixelformat = f->pixelformat;
 843	viacam_fmt_pre(upix, spix);
 844	v4l2_fill_mbus_format(&format.format, spix, f->mbus_code);
 845	ret = sensor_call(cam, pad, set_fmt, &pad_state, &format);
 846	v4l2_fill_pix_format(spix, &format.format);
 847	viacam_fmt_post(upix, spix);
 848	return ret;
 849}
 850
 851
 852
 853static int viacam_try_fmt_vid_cap(struct file *filp, void *priv,
 854		struct v4l2_format *fmt)
 855{
 856	struct via_camera *cam = video_drvdata(filp);
 857	struct v4l2_format sfmt;
 858
 859	return viacam_do_try_fmt(cam, &fmt->fmt.pix, &sfmt.fmt.pix);
 860}
 861
 862
 863static int viacam_g_fmt_vid_cap(struct file *filp, void *priv,
 864		struct v4l2_format *fmt)
 865{
 866	struct via_camera *cam = video_drvdata(filp);
 867
 868	fmt->fmt.pix = cam->user_format;
 869	return 0;
 870}
 871
 872static int viacam_s_fmt_vid_cap(struct file *filp, void *priv,
 873		struct v4l2_format *fmt)
 874{
 875	struct via_camera *cam = video_drvdata(filp);
 876	int ret;
 877	struct v4l2_format sfmt;
 878	struct via_format *f = via_find_format(fmt->fmt.pix.pixelformat);
 879
 880	/*
 881	 * Camera must be idle or we can't mess with the
 882	 * video setup.
 883	 */
 884	if (cam->opstate != S_IDLE)
 885		return -EBUSY;
 886	/*
 887	 * Let the sensor code look over and tweak the
 888	 * requested formatting.
 889	 */
 890	ret = viacam_do_try_fmt(cam, &fmt->fmt.pix, &sfmt.fmt.pix);
 891	if (ret)
 892		return ret;
 893	/*
 894	 * OK, let's commit to the new format.
 895	 */
 896	cam->user_format = fmt->fmt.pix;
 897	cam->sensor_format = sfmt.fmt.pix;
 898	cam->mbus_code = f->mbus_code;
 899	ret = viacam_configure_sensor(cam);
 900	if (!ret)
 901		ret = viacam_config_controller(cam);
 902	return ret;
 903}
 904
 905static int viacam_querycap(struct file *filp, void *priv,
 906		struct v4l2_capability *cap)
 907{
 908	strscpy(cap->driver, "via-camera", sizeof(cap->driver));
 909	strscpy(cap->card, "via-camera", sizeof(cap->card));
 910	strscpy(cap->bus_info, "platform:via-camera", sizeof(cap->bus_info));
 911	return 0;
 912}
 913
 914/* G/S_PARM */
 915
 916static int viacam_g_parm(struct file *filp, void *priv,
 917		struct v4l2_streamparm *parm)
 918{
 919	struct via_camera *cam = video_drvdata(filp);
 920
 921	return v4l2_g_parm_cap(video_devdata(filp), cam->sensor, parm);
 922}
 923
 924static int viacam_s_parm(struct file *filp, void *priv,
 925		struct v4l2_streamparm *parm)
 926{
 927	struct via_camera *cam = video_drvdata(filp);
 928
 929	return v4l2_s_parm_cap(video_devdata(filp), cam->sensor, parm);
 930}
 931
 932static int viacam_enum_framesizes(struct file *filp, void *priv,
 933		struct v4l2_frmsizeenum *sizes)
 934{
 935	unsigned int i;
 936
 937	if (sizes->index != 0)
 938		return -EINVAL;
 939	for (i = 0; i < N_VIA_FMTS; i++)
 940		if (sizes->pixel_format == via_formats[i].pixelformat)
 941			break;
 942	if (i >= N_VIA_FMTS)
 943		return -EINVAL;
 944	sizes->type = V4L2_FRMSIZE_TYPE_CONTINUOUS;
 945	sizes->stepwise.min_width = QCIF_WIDTH;
 946	sizes->stepwise.min_height = QCIF_HEIGHT;
 947	sizes->stepwise.max_width = VGA_WIDTH;
 948	sizes->stepwise.max_height = VGA_HEIGHT;
 949	sizes->stepwise.step_width = sizes->stepwise.step_height = 1;
 950	return 0;
 951}
 952
 953static int viacam_enum_frameintervals(struct file *filp, void *priv,
 954		struct v4l2_frmivalenum *interval)
 955{
 956	struct via_camera *cam = video_drvdata(filp);
 957	struct v4l2_subdev_frame_interval_enum fie = {
 958		.index = interval->index,
 959		.code = cam->mbus_code,
 960		.width = cam->sensor_format.width,
 961		.height = cam->sensor_format.height,
 962		.which = V4L2_SUBDEV_FORMAT_ACTIVE,
 963	};
 964	unsigned int i;
 965	int ret;
 966
 967	for (i = 0; i < N_VIA_FMTS; i++)
 968		if (interval->pixel_format == via_formats[i].pixelformat)
 969			break;
 970	if (i >= N_VIA_FMTS)
 971		return -EINVAL;
 972	if (interval->width < QCIF_WIDTH || interval->width > VGA_WIDTH ||
 973	    interval->height < QCIF_HEIGHT || interval->height > VGA_HEIGHT)
 974		return -EINVAL;
 975	ret = sensor_call(cam, pad, enum_frame_interval, NULL, &fie);
 976	if (ret)
 977		return ret;
 978	interval->type = V4L2_FRMIVAL_TYPE_DISCRETE;
 979	interval->discrete = fie.interval;
 980	return 0;
 981}
 982
 983static const struct v4l2_ioctl_ops viacam_ioctl_ops = {
 984	.vidioc_enum_input	= viacam_enum_input,
 985	.vidioc_g_input		= viacam_g_input,
 986	.vidioc_s_input		= viacam_s_input,
 987	.vidioc_enum_fmt_vid_cap = viacam_enum_fmt_vid_cap,
 988	.vidioc_try_fmt_vid_cap = viacam_try_fmt_vid_cap,
 989	.vidioc_g_fmt_vid_cap	= viacam_g_fmt_vid_cap,
 990	.vidioc_s_fmt_vid_cap	= viacam_s_fmt_vid_cap,
 991	.vidioc_querycap	= viacam_querycap,
 992	.vidioc_reqbufs		= vb2_ioctl_reqbufs,
 993	.vidioc_create_bufs	= vb2_ioctl_create_bufs,
 994	.vidioc_querybuf	= vb2_ioctl_querybuf,
 995	.vidioc_prepare_buf	= vb2_ioctl_prepare_buf,
 996	.vidioc_qbuf		= vb2_ioctl_qbuf,
 997	.vidioc_dqbuf		= vb2_ioctl_dqbuf,
 998	.vidioc_expbuf		= vb2_ioctl_expbuf,
 999	.vidioc_streamon	= vb2_ioctl_streamon,
1000	.vidioc_streamoff	= vb2_ioctl_streamoff,
1001	.vidioc_g_parm		= viacam_g_parm,
1002	.vidioc_s_parm		= viacam_s_parm,
1003	.vidioc_enum_framesizes = viacam_enum_framesizes,
1004	.vidioc_enum_frameintervals = viacam_enum_frameintervals,
1005	.vidioc_subscribe_event		= v4l2_ctrl_subscribe_event,
1006	.vidioc_unsubscribe_event	= v4l2_event_unsubscribe,
1007};
1008
1009/*----------------------------------------------------------------------------*/
1010
1011/*
1012 * Power management.
1013 */
1014#ifdef CONFIG_PM
1015
1016static int viacam_suspend(void *priv)
1017{
1018	struct via_camera *cam = priv;
1019	enum viacam_opstate state = cam->opstate;
1020
1021	if (cam->opstate != S_IDLE) {
1022		viacam_stop_engine(cam);
1023		cam->opstate = state; /* So resume restarts */
1024	}
1025
1026	return 0;
1027}
1028
1029static int viacam_resume(void *priv)
1030{
1031	struct via_camera *cam = priv;
1032	int ret = 0;
1033
1034	/*
1035	 * Get back to a reasonable operating state.
1036	 */
1037	via_write_reg_mask(VIASR, 0x78, 0, 0x80);
1038	via_write_reg_mask(VIASR, 0x1e, 0xc0, 0xc0);
1039	viacam_int_disable(cam);
1040	set_bit(CF_CONFIG_NEEDED, &cam->flags);
1041	/*
1042	 * Make sure the sensor's power state is correct
1043	 */
1044	if (!list_empty(&cam->vdev.fh_list))
1045		via_sensor_power_up(cam);
1046	else
1047		via_sensor_power_down(cam);
1048	/*
1049	 * If it was operating, try to restart it.
1050	 */
1051	if (cam->opstate != S_IDLE) {
1052		mutex_lock(&cam->lock);
1053		ret = viacam_configure_sensor(cam);
1054		if (!ret)
1055			ret = viacam_config_controller(cam);
1056		mutex_unlock(&cam->lock);
1057		if (!ret)
1058			viacam_start_engine(cam);
1059	}
1060
1061	return ret;
1062}
1063
1064static struct viafb_pm_hooks viacam_pm_hooks = {
1065	.suspend = viacam_suspend,
1066	.resume = viacam_resume
1067};
1068
1069#endif /* CONFIG_PM */
1070
1071/*
1072 * Setup stuff.
1073 */
1074
1075static const struct video_device viacam_v4l_template = {
1076	.name		= "via-camera",
1077	.minor		= -1,
1078	.fops		= &viacam_fops,
1079	.ioctl_ops	= &viacam_ioctl_ops,
1080	.release	= video_device_release_empty, /* Check this */
1081	.device_caps	= V4L2_CAP_VIDEO_CAPTURE | V4L2_CAP_READWRITE |
1082			  V4L2_CAP_STREAMING,
1083};
1084
1085/*
1086 * The OLPC folks put the serial port on the same pin as
1087 * the camera.	They also get grumpy if we break the
1088 * serial port and keep them from using it.  So we have
1089 * to check the serial enable bit and not step on it.
1090 */
1091#define VIACAM_SERIAL_DEVFN 0x88
1092#define VIACAM_SERIAL_CREG 0x46
1093#define VIACAM_SERIAL_BIT 0x40
1094
1095static bool viacam_serial_is_enabled(void)
1096{
1097	struct pci_bus *pbus = pci_find_bus(0, 0);
1098	u8 cbyte;
1099
1100	if (!pbus)
1101		return false;
1102	pci_bus_read_config_byte(pbus, VIACAM_SERIAL_DEVFN,
1103			VIACAM_SERIAL_CREG, &cbyte);
1104	if ((cbyte & VIACAM_SERIAL_BIT) == 0)
1105		return false; /* Not enabled */
1106	if (!override_serial) {
1107		printk(KERN_NOTICE "Via camera: serial port is enabled, " \
1108				"refusing to load.\n");
1109		printk(KERN_NOTICE "Specify override_serial=1 to force " \
1110				"module loading.\n");
1111		return true;
1112	}
1113	printk(KERN_NOTICE "Via camera: overriding serial port\n");
1114	pci_bus_write_config_byte(pbus, VIACAM_SERIAL_DEVFN,
1115			VIACAM_SERIAL_CREG, cbyte & ~VIACAM_SERIAL_BIT);
1116	return false;
1117}
1118
1119static struct ov7670_config sensor_cfg = {
1120	/* The XO-1.5 (only known user) clocks the camera at 90MHz. */
1121	.clock_speed = 90,
1122};
1123
1124static int viacam_probe(struct platform_device *pdev)
1125{
1126	int ret;
1127	struct i2c_adapter *sensor_adapter;
1128	struct viafb_dev *viadev = pdev->dev.platform_data;
1129	struct vb2_queue *vq;
1130	struct i2c_board_info ov7670_info = {
1131		.type = "ov7670",
1132		.addr = 0x42 >> 1,
1133		.platform_data = &sensor_cfg,
1134	};
1135
1136	/*
1137	 * Note that there are actually two capture channels on
1138	 * the device.	We only deal with one for now.	That
1139	 * is encoded here; nothing else assumes it's dealing with
1140	 * a unique capture device.
1141	 */
1142	struct via_camera *cam;
1143
1144	/*
1145	 * Ensure that frame buffer memory has been set aside for
1146	 * this purpose.  As an arbitrary limit, refuse to work
1147	 * with less than two frames of VGA 16-bit data.
1148	 *
1149	 * If we ever support the second port, we'll need to set
1150	 * aside more memory.
1151	 */
1152	if (viadev->camera_fbmem_size < (VGA_HEIGHT*VGA_WIDTH*4)) {
1153		printk(KERN_ERR "viacam: insufficient FB memory reserved\n");
1154		return -ENOMEM;
1155	}
1156	if (viadev->engine_mmio == NULL) {
1157		printk(KERN_ERR "viacam: No I/O memory, so no pictures\n");
1158		return -ENOMEM;
1159	}
1160
1161	if (machine_is_olpc() && viacam_serial_is_enabled())
1162		return -EBUSY;
1163
1164	/*
1165	 * Basic structure initialization.
1166	 */
1167	cam = kzalloc (sizeof(struct via_camera), GFP_KERNEL);
1168	if (cam == NULL)
1169		return -ENOMEM;
1170	via_cam_info = cam;
1171	cam->platdev = pdev;
1172	cam->viadev = viadev;
1173	cam->opstate = S_IDLE;
1174	cam->user_format = cam->sensor_format = viacam_def_pix_format;
1175	mutex_init(&cam->lock);
1176	INIT_LIST_HEAD(&cam->buffer_queue);
1177	cam->mmio = viadev->engine_mmio;
1178	cam->fbmem = viadev->fbmem;
1179	cam->fb_offset = viadev->camera_fbmem_offset;
1180	cam->flags = 1 << CF_CONFIG_NEEDED;
1181	cam->mbus_code = via_def_mbus_code;
1182	/*
1183	 * Tell V4L that we exist.
1184	 */
1185	ret = v4l2_device_register(&pdev->dev, &cam->v4l2_dev);
1186	if (ret) {
1187		dev_err(&pdev->dev, "Unable to register v4l2 device\n");
1188		goto out_free;
1189	}
1190	ret = v4l2_ctrl_handler_init(&cam->ctrl_handler, 10);
1191	if (ret)
1192		goto out_unregister;
1193	cam->v4l2_dev.ctrl_handler = &cam->ctrl_handler;
1194	/*
1195	 * Convince the system that we can do DMA.
1196	 */
1197	pdev->dev.dma_mask = &viadev->pdev->dma_mask;
1198	ret = dma_set_mask(&pdev->dev, 0xffffffff);
1199	if (ret)
1200		goto out_ctrl_hdl_free;
1201	/*
1202	 * Fire up the capture port.  The write to 0x78 looks purely
1203	 * OLPCish; any system will need to tweak 0x1e.
1204	 */
1205	via_write_reg_mask(VIASR, 0x78, 0, 0x80);
1206	via_write_reg_mask(VIASR, 0x1e, 0xc0, 0xc0);
1207	/*
1208	 * Get the sensor powered up.
1209	 */
1210	ret = via_sensor_power_setup(cam);
1211	if (ret)
1212		goto out_ctrl_hdl_free;
1213	via_sensor_power_up(cam);
1214
1215	/*
1216	 * See if we can't find it on the bus.	The VIA_PORT_31 assumption
1217	 * is OLPC-specific.  0x42 assumption is ov7670-specific.
1218	 */
1219	sensor_adapter = viafb_find_i2c_adapter(VIA_PORT_31);
1220	cam->sensor = v4l2_i2c_new_subdev_board(&cam->v4l2_dev, sensor_adapter,
1221			&ov7670_info, NULL);
1222	if (cam->sensor == NULL) {
1223		dev_err(&pdev->dev, "Unable to find the sensor!\n");
1224		ret = -ENODEV;
1225		goto out_power_down;
1226	}
1227	/*
1228	 * Get the IRQ.
1229	 */
1230	viacam_int_disable(cam);
1231	ret = request_threaded_irq(viadev->pdev->irq, viacam_quick_irq,
1232			viacam_irq, IRQF_SHARED, "via-camera", cam);
1233	if (ret)
1234		goto out_power_down;
1235
1236	vq = &cam->vq;
1237	vq->type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
1238	vq->io_modes = VB2_MMAP | VB2_USERPTR | VB2_DMABUF | VB2_READ;
1239	vq->drv_priv = cam;
1240	vq->timestamp_flags = V4L2_BUF_FLAG_TIMESTAMP_MONOTONIC;
1241	vq->buf_struct_size = sizeof(struct via_buffer);
1242	vq->dev = cam->v4l2_dev.dev;
1243
1244	vq->ops = &viacam_vb2_ops;
1245	vq->mem_ops = &vb2_dma_sg_memops;
1246	vq->lock = &cam->lock;
1247
1248	ret = vb2_queue_init(vq);
1249	/*
1250	 * Tell V4l2 that we exist.
1251	 */
1252	cam->vdev = viacam_v4l_template;
1253	cam->vdev.v4l2_dev = &cam->v4l2_dev;
1254	cam->vdev.lock = &cam->lock;
1255	cam->vdev.queue = vq;
1256	video_set_drvdata(&cam->vdev, cam);
1257	ret = video_register_device(&cam->vdev, VFL_TYPE_VIDEO, -1);
1258	if (ret)
1259		goto out_irq;
1260
1261#ifdef CONFIG_PM
1262	/*
1263	 * Hook into PM events
1264	 */
1265	viacam_pm_hooks.private = cam;
1266	viafb_pm_register(&viacam_pm_hooks);
1267#endif
1268
1269	/* Power the sensor down until somebody opens the device */
1270	via_sensor_power_down(cam);
1271	return 0;
1272
1273out_irq:
1274	free_irq(viadev->pdev->irq, cam);
1275out_power_down:
1276	via_sensor_power_release(cam);
1277out_ctrl_hdl_free:
1278	v4l2_ctrl_handler_free(&cam->ctrl_handler);
1279out_unregister:
1280	v4l2_device_unregister(&cam->v4l2_dev);
1281out_free:
1282	kfree(cam);
1283	return ret;
1284}
1285
1286static void viacam_remove(struct platform_device *pdev)
1287{
1288	struct via_camera *cam = via_cam_info;
1289	struct viafb_dev *viadev = pdev->dev.platform_data;
1290
1291	video_unregister_device(&cam->vdev);
1292	v4l2_device_unregister(&cam->v4l2_dev);
1293#ifdef CONFIG_PM
1294	viafb_pm_unregister(&viacam_pm_hooks);
1295#endif
1296	free_irq(viadev->pdev->irq, cam);
1297	via_sensor_power_release(cam);
1298	v4l2_ctrl_handler_free(&cam->ctrl_handler);
1299	kfree(cam);
1300	via_cam_info = NULL;
1301}
1302
1303static struct platform_driver viacam_driver = {
1304	.driver = {
1305		.name = "viafb-camera",
1306	},
1307	.probe = viacam_probe,
1308	.remove = viacam_remove,
1309};
1310
1311module_platform_driver(viacam_driver);