V4L/DVB (3376): Add cpia2 camera support

There has been a CPIA2 driver out of kernel for a long time and it has
been pretty clean for some time too. This is an import of the
sourceforge driver which has been stripped of
- 2.4 back compatibility
- 2.4 old style MJPEG ioctls
A couple of functions have been made static and the docs have been
repackaged into Documentation/video4linux.  The rvmalloc/free functions now
match the cpia driver again.  Other than that this is the code as is.
Tested on x86-64 with a QX5 microscope.

Signed-off-by: Alan Cox <alan@redhat.com>
Signed-off-by: Andrew Morton <akpm@osdl.org>
Signed-off-by: Mauro Carvalho Chehab <mchehab@infradead.org>

12 files changed, 6974 insertions, 0 deletions

diff --git a/Documentation/video4linux/README.cpia2 b/Documentation/video4linux/README.cpia2
new file mode 100644
index 00000000000000..f3bd3439ad913b
--- /dev/null
+++ b/Documentation/video4linux/README.cpia2
@@ -0,0 +1,130 @@
+$Id: README,v 1.7 2005/08/29 23:39:57 sbertin Exp $
+
+1. Introduction
+
+	This is a driver for STMicroelectronics's CPiA2 (second generation
+Colour Processor Interface ASIC) based cameras. This camera outputs an MJPEG
+stream at up to vga size. It implements the Video4Linux interface as much as
+possible.  Since the V4L interface does not support compressed formats, only
+an mjpeg enabled application can be used with the camera. We have modified the
+gqcam application to view this stream.
+
+	The driver is implemented as two kernel modules. The cpia2 module
+contains the camera functions and the V4L interface.  The cpia2_usb module
+contains usb specific functions.  The main reason for this was the size of the
+module was getting out of hand, so I separted them.  It is not likely that
+there will be a parallel port version.
+
+FEATURES:
+   - Supports cameras with the Vision stv6410 (CIF) and stv6500 (VGA) cmos
+     sensors. I only have the vga sensor, so can't test the other.
+   - Image formats: VGA, QVGA, CIF, QCIF, and a number of sizes in between.
+     VGA and QVGA are the native image sizes for the VGA camera. CIF is done
+     in the coprocessor by scaling QVGA.  All other sizes are done by clipping.
+   - Palette: YCrCb, compressed with MJPEG.
+   - Some compression parameters are settable.
+   - Sensor framerate is adjustable (up to 30 fps CIF, 15 fps VGA).
+   - Adjust brightness, color, contrast while streaming.
+   - Flicker control settable for 50 or 60 Hz mains frequency.
+
+2. Making and installing the stv672 driver modules:
+
+	Requirements:
+	-------------
+	This should work with 2.4 (2.4.23 and later) and 2.6 kernels, but has
+only been tested on 2.6.  Video4Linux must be either compiled into the kernel or
+available as a module.  Video4Linux2 is automatically detected and made
+available at compile time.
+
+	Compiling:
+	----------
+	As root, do a make install.  This will compile and install the modules
+into the media/video directory in the module tree. For 2.4 kernels, use
+Makefile_2.4 (aka do make -f Makefile_2.4 install).
+
+	Setup:
+	------
+	Use 'modprobe cpia2' to load and 'modprobe -r cpia2' to unload. This
+may be done automatically by your distribution.
+
+3. Driver options
+
+	Option		Description
+	------		-----------
+	video_nr	video device to register (0=/dev/video0, etc)
+			range -1 to 64.  default is -1 (first available)
+			If you have more than 1 camera, this MUST be -1.
+	buffer_size	Size for each frame buffer in bytes (default 68k)
+	num_buffers	Number of frame buffers (1-32, default 3)
+	alternate	USB Alternate (2-7, default 7)
+	flicker_freq	Frequency for flicker reduction(50 or 60, default 60)
+	flicker_mode	0 to disable, or 1 to enable flicker reduction.
+			(default 0). This is only effective if the camera
+			uses a stv0672 coprocessor.
+
+	Setting the options:
+	--------------------
+	If you are using modules, edit /etc/modules.conf and add an options
+line like this:
+	options cpia2 num_buffers=3 buffer_size=65535
+
+	If the driver is compiled into the kernel, at boot time specify them
+like this:
+	cpia2=num_buffers:3,buffer_size:65535
+
+	What buffer size should I use?
+	------------------------------
+	The maximum image size depends on the alternate you choose, and the
+frame rate achieved by the camera.  If the compression engine is able to
+keep up with the frame rate, the maximum image size is given by the table
+below.
+	The compression engine starts out at maximum compression, and will
+increase image quality until it is close to the size in the table.  As long
+as the compression engine can keep up with the frame rate, after a short time
+the images will all be about the size in the table, regardless of resolution.
+	At low alternate settings, the compression engine may not be able to
+compress the image enough and will reduce the frame rate by producing larger
+images.
+	The default of 68k should be good for most users.  This will handle
+any alternate at frame rates down to 15fps.  For lower frame rates, it may
+be necessary to increase the buffer size to avoid having frames dropped due
+to insufficient space.
+
+			     Image size(bytes)
+	Alternate  bytes/ms   15fps    30fps
+	    2         128      8533     4267
+	    3         384     25600    12800
+	    4         640     42667    21333
+	    5         768     51200    25600
+	    6         896     59733    29867
+	    7        1023     68200    34100
+
+	How many buffers should I use?
+	------------------------------
+	For normal streaming, 3 should give the best results.  With only 2,
+it is possible for the camera to finish sending one image just after a
+program has started reading the other.  If this happens, the driver must drop
+a frame.  The exception to this is if you have a heavily loaded machine.  In
+this case use 2 buffers.  You are probably not reading at the full frame rate.
+If the camera can send multiple images before a read finishes, it could
+overwrite the third buffer before the read finishes, leading to a corrupt
+image.  Single and double buffering have extra checks to avoid overwriting.
+
+4. Using the camera
+
+	We are providing a modified gqcam application to view the output. In
+order to avoid confusion, here it is called mview.  There is also the qx5view
+program which can also control the lights on the qx5 microscope. MJPEG Tools
+(http://mjpeg.sourceforge.net) can also be used to record from the camera.
+
+5. Notes to developers:
+
+   - This is a driver version stripped of the 2.4 back compatibility
+     and old MJPEG ioctl API. See cpia2.sf.net for 2.4 support.
+
+6. Thanks:
+
+   - Peter Pregler <Peter_Pregler@email.com>,
+     Scott J. Bertin <scottbertin@yahoo.com>, and
+     Jarl Totland <Jarl.Totland@bdc.no> for the original cpia driver, which
+     this one was modelled from.
diff --git a/Documentation/video4linux/cpia2_overview.txt b/Documentation/video4linux/cpia2_overview.txt
new file mode 100644
index 00000000000000..a6e53665216bc8
--- /dev/null
+++ b/Documentation/video4linux/cpia2_overview.txt
@@ -0,0 +1,38 @@
+			Programmer's View of Cpia2
+
+Cpia2 is the second generation video coprocessor from VLSI Vision Ltd (now a
+division of ST Microelectronics).  There are two versions.  The first is the
+STV0672, which is capable of up to 30 frames per second (fps) in frame sizes
+up to CIF, and 15 fps for VGA frames.  The STV0676 is an improved version,
+which can handle up to 30 fps VGA.  Both coprocessors can be attached to two
+CMOS sensors - the vvl6410 CIF sensor and the vvl6500 VGA sensor.  These will
+be referred to as the 410 and the 500 sensors, or the CIF and VGA sensors.
+
+The two chipsets operate almost identically.  The core is an 8051 processor,
+running two different versions of firmware.  The 672 runs the VP4 video
+processor code, the 676 runs VP5.  There are a few differences in register
+mappings for the two chips.  In these cases, the symbols defined in the
+header files are marked with VP4 or VP5 as part of the symbol name.
+
+The cameras appear externally as three sets of registers. Setting register
+values is the only way to control the camera.  Some settings are
+interdependant, such as the sequence required to power up the camera. I will
+try to make note of all of these cases.
+
+The register sets are called blocks.  Block 0 is the system block.  This
+section is always powered on when the camera is plugged in.  It contains
+registers that control housekeeping functions such as powering up the video
+processor.  The video processor is the VP block.  These registers control
+how the video from the sensor is processed.  Examples are timing registers,
+user mode (vga, qvga), scaling, cropping, framerates, and so on.  The last
+block is the video compressor (VC).  The video stream sent from the camera is
+compressed as Motion JPEG (JPEGA).  The VC controls all of the compression
+parameters.  Looking at the file cpia2_registers.h, you can get a full view
+of these registers and the possible values for most of them.
+
+One or more registers can be set or read by sending a usb control message to
+the camera.  There are three modes for this.  Block mode requests a number
+of contiguous registers.  Random mode reads or writes random registers with
+a tuple structure containing address/value pairs.  The repeat mode is only
+used by VP4 to load a firmware patch.  It contains a starting address and
+a sequence of bytes to be written into a gpio port.
\ No newline at end of file
diff --git a/drivers/media/video/Kconfig b/drivers/media/video/Kconfig
index d82c8a30ba4437..f6889f771053d1 100644
--- a/drivers/media/video/Kconfig
+++ b/drivers/media/video/Kconfig
@@ -142,6 +142,16 @@ config VIDEO_CPIA_USB
 	  otherwise say N. This will not work with the Creative Webcam III.
 	  It is also available as a module (cpia_usb).
 
+config VIDEO_CPIA2
+	tristate "CPiA2 Video For Linux"
+	depends on VIDEO_DEV
+	---help---
+	  This is the video4linux driver for cameras based on Vision's CPiA2
+	  (Colour Processor Interface ASIC), such as the Digital Blue QX5
+	  Microscope. If you have one of these cameras, say Y here
+
+	  This driver is also available as a module (cpia2).
+
 config VIDEO_SAA5246A
 	tristate "SAA5246A, SAA5281 Teletext processor"
 	depends on VIDEO_DEV && I2C
diff --git a/drivers/media/video/Makefile b/drivers/media/video/Makefile
index faf728366c4e4e..87b1ce6e2050a8 100644
--- a/drivers/media/video/Makefile
+++ b/drivers/media/video/Makefile
@@ -47,6 +47,7 @@ obj-$(CONFIG_VIDEO_EM28XX) += em28xx/
 obj-$(CONFIG_VIDEO_EM28XX) += saa711x.o tvp5150.o
 obj-$(CONFIG_VIDEO_AUDIO_DECODER) += wm8775.o cs53l32a.o
 obj-$(CONFIG_VIDEO_OVCAMCHIP) += ovcamchip/
+obj-$(CONFIG_VIDEO_CPIA2) += cpia2/
 obj-$(CONFIG_VIDEO_MXB) += saa7111.o tuner.o tda9840.o tea6415c.o tea6420.o mxb.o
 obj-$(CONFIG_VIDEO_HEXIUM_ORION) += hexium_orion.o
 obj-$(CONFIG_VIDEO_HEXIUM_GEMINI) += hexium_gemini.o
diff --git a/drivers/media/video/cpia2/Makefile b/drivers/media/video/cpia2/Makefile
new file mode 100644
index 00000000000000..828cf1b1df860a
--- /dev/null
+++ b/drivers/media/video/cpia2/Makefile
@@ -0,0 +1,3 @@
+cpia2-objs	:= cpia2_v4l.o cpia2_usb.o cpia2_core.o
+
+obj-$(CONFIG_VIDEO_CPIA2) += cpia2.o
diff --git a/drivers/media/video/cpia2/cpia2.h b/drivers/media/video/cpia2/cpia2.h
new file mode 100644
index 00000000000000..95d3afa94a3d92
--- /dev/null
+++ b/drivers/media/video/cpia2/cpia2.h
@@ -0,0 +1,497 @@
+/****************************************************************************
+ *
+ *  Filename: cpia2.h
+ *
+ *  Copyright 2001, STMicrolectronics, Inc.
+ *
+ *  Contact:  steve.miller@st.com
+ *
+ *  Description:
+ *     This is a USB driver for CPiA2 based video cameras.
+ *
+ *     This driver is modelled on the cpia usb driver by
+ *     Jochen Scharrlach and Johannes Erdfeldt.
+ *
+ *  This program is free software; you can redistribute it and/or modify
+ *  it under the terms of the GNU General Public License as published by
+ *  the Free Software Foundation; either version 2 of the License, or
+ *  (at your option) any later version.
+ *
+ *  This program is distributed in the hope that it will be useful,
+ *  but WITHOUT ANY WARRANTY; without even the implied warranty of
+ *  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ *  GNU General Public License for more details.
+ *
+ *  You should have received a copy of the GNU General Public License
+ *  along with this program; if not, write to the Free Software
+ *  Foundation, Inc., 675 Mass Ave, Cambridge, MA 02139, USA.
+ *
+ ****************************************************************************/
+
+#ifndef __CPIA2_H__
+#define __CPIA2_H__
+
+#include <linux/version.h>
+#include <linux/videodev.h>
+#include <linux/usb.h>
+#include <linux/poll.h>
+
+#include "cpia2dev.h"
+#include "cpia2_registers.h"
+
+/* define for verbose debug output */
+//#define _CPIA2_DEBUG_
+
+#define CPIA2_MAJ_VER	2
+#define CPIA2_MIN_VER   0
+#define CPIA2_PATCH_VER	0
+
+/***
+ * Image defines
+ ***/
+#ifndef true
+#define true 1
+#define false 0
+#endif
+
+/*  Misc constants */
+#define ALLOW_CORRUPT 0		/* Causes collater to discard checksum */
+
+/* USB Transfer mode */
+#define XFER_ISOC 0
+#define XFER_BULK 1
+
+/* USB Alternates */
+#define USBIF_CMDONLY 0
+#define USBIF_BULK 1
+#define USBIF_ISO_1 2	/*  128 bytes/ms */
+#define USBIF_ISO_2 3	/*  384 bytes/ms */
+#define USBIF_ISO_3 4	/*  640 bytes/ms */
+#define USBIF_ISO_4 5	/*  768 bytes/ms */
+#define USBIF_ISO_5 6	/*  896 bytes/ms */
+#define USBIF_ISO_6 7	/* 1023 bytes/ms */
+
+/* Flicker Modes */
+#define NEVER_FLICKER   0
+#define ANTI_FLICKER_ON 1
+#define FLICKER_60      60
+#define FLICKER_50      50
+
+/* Debug flags */
+#define DEBUG_NONE          0
+#define DEBUG_REG           0x00000001
+#define DEBUG_DUMP_PATCH    0x00000002
+#define DEBUG_DUMP_REGS     0x00000004
+
+/***
+ * Video frame sizes
+ ***/
+enum {
+	VIDEOSIZE_VGA = 0,	/* 640x480 */
+	VIDEOSIZE_CIF,		/* 352x288 */
+	VIDEOSIZE_QVGA,		/* 320x240 */
+	VIDEOSIZE_QCIF,		/* 176x144 */
+	VIDEOSIZE_288_216,
+	VIDEOSIZE_256_192,
+	VIDEOSIZE_224_168,
+	VIDEOSIZE_192_144,
+};
+
+#define STV_IMAGE_CIF_ROWS    288
+#define STV_IMAGE_CIF_COLS    352
+
+#define STV_IMAGE_QCIF_ROWS   144
+#define STV_IMAGE_QCIF_COLS   176
+
+#define STV_IMAGE_VGA_ROWS    480
+#define STV_IMAGE_VGA_COLS    640
+
+#define STV_IMAGE_QVGA_ROWS   240
+#define STV_IMAGE_QVGA_COLS   320
+
+#define JPEG_MARKER_COM (1<<6)	/* Comment segment */
+
+/***
+ * Enums
+ ***/
+/* Sensor types available with cpia2 asics */
+enum sensors {
+	CPIA2_SENSOR_410,
+	CPIA2_SENSOR_500
+};
+
+/* Asic types available in the CPiA2 architecture */
+#define  CPIA2_ASIC_672 0x67
+
+/* Device types (stv672, stv676, etc) */
+#define  DEVICE_STV_672   0x0001
+#define  DEVICE_STV_676   0x0002
+
+enum frame_status {
+	FRAME_EMPTY,
+	FRAME_READING,		/* In the process of being grabbed into */
+	FRAME_READY,		/* Ready to be read */
+	FRAME_ERROR,
+};
+
+/***
+ * Register access (for USB request byte)
+ ***/
+enum {
+	CAMERAACCESS_SYSTEM = 0,
+	CAMERAACCESS_VC,
+	CAMERAACCESS_VP,
+	CAMERAACCESS_IDATA
+};
+
+#define CAMERAACCESS_TYPE_BLOCK    0x00
+#define CAMERAACCESS_TYPE_RANDOM   0x04
+#define CAMERAACCESS_TYPE_MASK     0x08
+#define CAMERAACCESS_TYPE_REPEAT   0x0C
+
+#define TRANSFER_READ 0
+#define TRANSFER_WRITE 1
+
+#define DEFAULT_ALT   USBIF_ISO_6
+#define DEFAULT_BRIGHTNESS 0x46
+#define DEFAULT_CONTRAST 0x93
+#define DEFAULT_SATURATION 0x7f
+#define DEFAULT_TARGET_KB 0x30
+
+/* Power state */
+#define HI_POWER_MODE CPIA2_SYSTEM_CONTROL_HIGH_POWER
+#define LO_POWER_MODE CPIA2_SYSTEM_CONTROL_LOW_POWER
+
+
+/********
+ * Commands
+ *******/
+enum {
+	CPIA2_CMD_NONE = 0,
+	CPIA2_CMD_GET_VERSION,
+	CPIA2_CMD_GET_PNP_ID,
+	CPIA2_CMD_GET_ASIC_TYPE,
+	CPIA2_CMD_GET_SENSOR,
+	CPIA2_CMD_GET_VP_DEVICE,
+	CPIA2_CMD_GET_VP_BRIGHTNESS,
+	CPIA2_CMD_SET_VP_BRIGHTNESS,
+	CPIA2_CMD_GET_CONTRAST,
+	CPIA2_CMD_SET_CONTRAST,
+	CPIA2_CMD_GET_VP_SATURATION,
+	CPIA2_CMD_SET_VP_SATURATION,
+	CPIA2_CMD_GET_VP_GPIO_DIRECTION,
+	CPIA2_CMD_SET_VP_GPIO_DIRECTION,
+	CPIA2_CMD_GET_VP_GPIO_DATA,
+	CPIA2_CMD_SET_VP_GPIO_DATA,
+	CPIA2_CMD_GET_VC_MP_GPIO_DIRECTION,
+	CPIA2_CMD_SET_VC_MP_GPIO_DIRECTION,
+	CPIA2_CMD_GET_VC_MP_GPIO_DATA,
+	CPIA2_CMD_SET_VC_MP_GPIO_DATA,
+	CPIA2_CMD_ENABLE_PACKET_CTRL,
+	CPIA2_CMD_GET_FLICKER_MODES,
+	CPIA2_CMD_SET_FLICKER_MODES,
+	CPIA2_CMD_RESET_FIFO,	/* clear fifo and enable stream block */
+	CPIA2_CMD_SET_HI_POWER,
+	CPIA2_CMD_SET_LOW_POWER,
+	CPIA2_CMD_CLEAR_V2W_ERR,
+	CPIA2_CMD_SET_USER_MODE,
+	CPIA2_CMD_GET_USER_MODE,
+	CPIA2_CMD_FRAMERATE_REQ,
+	CPIA2_CMD_SET_COMPRESSION_STATE,
+	CPIA2_CMD_GET_WAKEUP,
+	CPIA2_CMD_SET_WAKEUP,
+	CPIA2_CMD_GET_PW_CONTROL,
+	CPIA2_CMD_SET_PW_CONTROL,
+	CPIA2_CMD_GET_SYSTEM_CTRL,
+	CPIA2_CMD_SET_SYSTEM_CTRL,
+	CPIA2_CMD_GET_VP_SYSTEM_STATE,
+	CPIA2_CMD_GET_VP_SYSTEM_CTRL,
+	CPIA2_CMD_SET_VP_SYSTEM_CTRL,
+	CPIA2_CMD_GET_VP_EXP_MODES,
+	CPIA2_CMD_SET_VP_EXP_MODES,
+	CPIA2_CMD_GET_DEVICE_CONFIG,
+	CPIA2_CMD_SET_DEVICE_CONFIG,
+	CPIA2_CMD_SET_SERIAL_ADDR,
+	CPIA2_CMD_SET_SENSOR_CR1,
+	CPIA2_CMD_GET_VC_CONTROL,
+	CPIA2_CMD_SET_VC_CONTROL,
+	CPIA2_CMD_SET_TARGET_KB,
+	CPIA2_CMD_SET_DEF_JPEG_OPT,
+	CPIA2_CMD_REHASH_VP4,
+	CPIA2_CMD_GET_USER_EFFECTS,
+	CPIA2_CMD_SET_USER_EFFECTS
+};
+
+enum user_cmd {
+	COMMAND_NONE = 0x00000001,
+	COMMAND_SET_FPS = 0x00000002,
+	COMMAND_SET_COLOR_PARAMS = 0x00000004,
+	COMMAND_GET_COLOR_PARAMS = 0x00000008,
+	COMMAND_SET_FORMAT = 0x00000010,	/* size, etc */
+	COMMAND_SET_FLICKER = 0x00000020
+};
+
+/***
+ * Some defines specific to the 676 chip
+ ***/
+#define CAMACC_CIF      0x01
+#define CAMACC_VGA      0x02
+#define CAMACC_QCIF     0x04
+#define CAMACC_QVGA     0x08
+
+
+struct cpia2_register {
+	u8 index;
+	u8 value;
+};
+
+struct cpia2_reg_mask {
+	u8 index;
+	u8 and_mask;
+	u8 or_mask;
+	u8 fill;
+};
+
+struct cpia2_command {
+	u32 command;
+	u8 req_mode;		/* (Block or random) | registerBank */
+	u8 reg_count;
+	u8 direction;
+	u8 start;
+	union reg_types {
+		struct cpia2_register registers[32];
+		struct cpia2_reg_mask masks[16];
+		u8 block_data[64];
+		u8 *patch_data;	/* points to function defined block */
+	} buffer;
+};
+
+struct camera_params {
+	struct {
+		u8 firmware_revision_hi; /* For system register set (bank 0) */
+		u8 firmware_revision_lo;
+		u8 asic_id;	/* Video Compressor set (bank 1) */
+		u8 asic_rev;
+		u8 vp_device_hi;	/* Video Processor set (bank 2) */
+		u8 vp_device_lo;
+		u8 sensor_flags;
+		u8 sensor_rev;
+	} version;
+
+	struct {
+		u32 device_type;     /* enumerated from vendor/product ids.
+				      * Currently, either STV_672 or STV_676 */
+		u16 vendor;
+		u16 product;
+		u16 device_revision;
+	} pnp_id;
+
+	struct {
+		u8 brightness;	/* CPIA2_VP_EXPOSURE_TARGET */
+		u8 contrast;	/* Note: this is CPIA2_VP_YRANGE */
+		u8 saturation;	/*  CPIA2_VP_SATURATION */
+	} color_params;
+
+	struct {
+		u8 cam_register;
+		u8 flicker_mode_req;	/* 1 if flicker on, else never flicker */
+		int mains_frequency;
+	} flicker_control;
+
+	struct {
+		u8 jpeg_options;
+		u8 creep_period;
+		u8 user_squeeze;
+		u8 inhibit_htables;
+	} compression;
+
+	struct {
+		u8 ohsize;	/* output image size */
+		u8 ovsize;
+		u8 hcrop;	/* cropping start_pos/4 */
+		u8 vcrop;
+		u8 hphase;	/* scaling registers */
+		u8 vphase;
+		u8 hispan;
+		u8 vispan;
+		u8 hicrop;
+		u8 vicrop;
+		u8 hifraction;
+		u8 vifraction;
+	} image_size;
+
+	struct {
+		int width;	/* actual window width */
+		int height;	/* actual window height */
+	} roi;
+
+	struct {
+		u8 video_mode;
+		u8 frame_rate;
+		u8 video_size;	/* Not a register, just a convenience for cropped sizes */
+		u8 gpio_direction;
+		u8 gpio_data;
+		u8 system_ctrl;
+		u8 system_state;
+		u8 lowlight_boost;	/* Bool: 0 = off, 1 = on */
+		u8 device_config;
+		u8 exposure_modes;
+		u8 user_effects;
+	} vp_params;
+
+	struct {
+		u8 pw_control;
+		u8 wakeup;
+		u8 vc_control;
+		u8 vc_mp_direction;
+		u8 vc_mp_data;
+		u8 target_kb;
+	} vc_params;
+
+	struct {
+		u8 power_mode;
+		u8 system_ctrl;
+		u8 stream_mode;	/* This is the current alternate for usb drivers */
+		u8 allow_corrupt;
+	} camera_state;
+};
+
+#define NUM_SBUF    2
+
+struct cpia2_sbuf {
+	char *data;
+	struct urb *urb;
+};
+
+struct framebuf {
+	struct timeval timestamp;
+	unsigned long seq;
+	int num;
+	int length;
+	int max_length;
+	volatile enum frame_status status;
+	u8 *data;
+	struct framebuf *next;
+};
+
+struct cpia2_fh {
+	enum v4l2_priority prio;
+	u8 mmapped;
+};
+
+struct camera_data {
+	/* locks */
+	struct semaphore busy_lock;	/* guard against SMP multithreading */
+	struct v4l2_prio_state prio;
+
+	/* camera status */
+	volatile int present;	/* Is the camera still present? */
+	int open_count;		/* # of process that have camera open */
+	int first_image_seen;
+	u8 mains_freq;		/* for flicker control */
+	enum sensors sensor_type;
+	u8 flush;
+	u8 mmapped;
+	int streaming;		/* 0 = no, 1 = yes */
+	int xfer_mode;		/* XFER_BULK or XFER_ISOC */
+	struct camera_params params;	/* camera settings */
+
+	/* v4l */
+	int video_size;			/* VIDEO_SIZE_ */
+	struct video_device *vdev;	/* v4l videodev */
+	struct video_picture vp;	/* v4l camera settings */
+	struct video_window vw;		/* v4l capture area */
+	__u32 pixelformat;       /* Format fourcc      */
+
+	/* USB */
+	struct usb_device *dev;
+	unsigned char iface;
+	unsigned int cur_alt;
+	unsigned int old_alt;
+	struct cpia2_sbuf sbuf[NUM_SBUF];	/* Double buffering */
+
+	wait_queue_head_t wq_stream;
+
+	/* Buffering */
+	u32 frame_size;
+	int num_frames;
+	unsigned long frame_count;
+	u8 *frame_buffer;	/* frame buffer data */
+	struct framebuf *buffers;
+	struct framebuf * volatile curbuff;
+	struct framebuf *workbuff;
+
+	/* MJPEG Extension */
+	int APPn;		/* Number of APP segment to be written, must be 0..15 */
+	int APP_len;		/* Length of data in JPEG APPn segment */
+	char APP_data[60];	/* Data in the JPEG APPn segment. */
+
+	int COM_len;		/* Length of data in JPEG COM segment */
+	char COM_data[60];	/* Data in JPEG COM segment */
+};
+
+/* v4l */
+int cpia2_register_camera(struct camera_data *cam);
+void cpia2_unregister_camera(struct camera_data *cam);
+
+/* core */
+int cpia2_reset_camera(struct camera_data *cam);
+int cpia2_set_low_power(struct camera_data *cam);
+void cpia2_dbg_dump_registers(struct camera_data *cam);
+int cpia2_match_video_size(int width, int height);
+void cpia2_set_camera_state(struct camera_data *cam);
+void cpia2_save_camera_state(struct camera_data *cam);
+void cpia2_set_color_params(struct camera_data *cam);
+void cpia2_set_brightness(struct camera_data *cam, unsigned char value);
+void cpia2_set_contrast(struct camera_data *cam, unsigned char value);
+void cpia2_set_saturation(struct camera_data *cam, unsigned char value);
+int cpia2_set_flicker_mode(struct camera_data *cam, int mode);
+void cpia2_set_format(struct camera_data *cam);
+int cpia2_send_command(struct camera_data *cam, struct cpia2_command *cmd);
+int cpia2_do_command(struct camera_data *cam,
+		     unsigned int command,
+		     unsigned char direction, unsigned char param);
+struct camera_data *cpia2_init_camera_struct(void);
+int cpia2_init_camera(struct camera_data *cam);
+int cpia2_allocate_buffers(struct camera_data *cam);
+void cpia2_free_buffers(struct camera_data *cam);
+long cpia2_read(struct camera_data *cam,
+		char *buf, unsigned long count, int noblock);
+unsigned int cpia2_poll(struct camera_data *cam,
+			struct file *filp, poll_table *wait);
+int cpia2_remap_buffer(struct camera_data *cam, struct vm_area_struct *vma);
+void cpia2_set_property_flip(struct camera_data *cam, int prop_val);
+void cpia2_set_property_mirror(struct camera_data *cam, int prop_val);
+int cpia2_set_target_kb(struct camera_data *cam, unsigned char value);
+int cpia2_set_gpio(struct camera_data *cam, unsigned char setting);
+int cpia2_set_fps(struct camera_data *cam, int framerate);
+
+/* usb */
+int cpia2_usb_init(void);
+void cpia2_usb_cleanup(void);
+int cpia2_usb_transfer_cmd(struct camera_data *cam, void *registers,
+			   u8 request, u8 start, u8 count, u8 direction);
+int cpia2_usb_stream_start(struct camera_data *cam, unsigned int alternate);
+int cpia2_usb_stream_stop(struct camera_data *cam);
+int cpia2_usb_stream_pause(struct camera_data *cam);
+int cpia2_usb_stream_resume(struct camera_data *cam);
+int cpia2_usb_change_streaming_alternate(struct camera_data *cam,
+					 unsigned int alt);
+
+
+/* ----------------------- debug functions ---------------------- */
+#ifdef _CPIA2_DEBUG_
+#define ALOG(lev, fmt, args...) printk(lev "%s:%d %s(): " fmt, __FILE__, __LINE__, __func__, ## args)
+#define LOG(fmt, args...) ALOG(KERN_INFO, fmt, ## args)
+#define ERR(fmt, args...) ALOG(KERN_ERR, fmt, ## args)
+#define DBG(fmt, args...) ALOG(KERN_DEBUG, fmt, ## args)
+#else
+#define ALOG(fmt,args...) printk(fmt,##args)
+#define LOG(fmt,args...) ALOG(KERN_INFO "cpia2: "fmt,##args)
+#define ERR(fmt,args...) ALOG(KERN_ERR "cpia2: "fmt,##args)
+#define DBG(fmn,args...) do {} while(0)
+#endif
+/* No function or lineno, for shorter lines */
+#define KINFO(fmt, args...) printk(KERN_INFO fmt,##args)
+
+#endif
diff --git a/drivers/media/video/cpia2/cpia2_core.c b/drivers/media/video/cpia2/cpia2_core.c
new file mode 100644
index 00000000000000..5dfb242d5b8c5d
--- /dev/null
+++ b/drivers/media/video/cpia2/cpia2_core.c
@@ -0,0 +1,2525 @@
+/****************************************************************************
+ *
+ *  Filename: cpia2_core.c
+ *
+ *  Copyright 2001, STMicrolectronics, Inc.
+ *      Contact:  steve.miller@st.com
+ *
+ *  Description:
+ *     This is a USB driver for CPia2 based video cameras.
+ *     The infrastructure of this driver is based on the cpia usb driver by
+ *     Jochen Scharrlach and Johannes Erdfeldt.
+ *
+ *  This program is free software; you can redistribute it and/or modify
+ *  it under the terms of the GNU General Public License as published by
+ *  the Free Software Foundation; either version 2 of the License, or
+ *  (at your option) any later version.
+ *
+ *  This program is distributed in the hope that it will be useful,
+ *  but WITHOUT ANY WARRANTY; without even the implied warranty of
+ *  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ *  GNU General Public License for more details.
+ *
+ *  You should have received a copy of the GNU General Public License
+ *  along with this program; if not, write to the Free Software
+ *  Foundation, Inc., 675 Mass Ave, Cambridge, MA 02139, USA.
+ *
+ *  Stripped of 2.4 stuff ready for main kernel submit by
+ *		Alan Cox <alan@redhat.com>
+ *
+ ****************************************************************************/
+
+#include "cpia2.h"
+
+#include <linux/slab.h>
+#include <linux/vmalloc.h>
+
+//#define _CPIA2_DEBUG_
+
+#include "cpia2patch.h"
+
+#ifdef _CPIA2_DEBUG_
+
+static const char *block_name[] = {
+	"System",
+	"VC",
+	"VP",
+	"IDATA"
+};
+#endif
+
+static unsigned int debugs_on = 0;//DEBUG_REG;
+
+
+/******************************************************************************
+ *
+ *  Forward Declarations
+ *
+ *****************************************************************************/
+static int apply_vp_patch(struct camera_data *cam);
+static int set_default_user_mode(struct camera_data *cam);
+static int set_vw_size(struct camera_data *cam, int size);
+static int configure_sensor(struct camera_data *cam,
+			    int reqwidth, int reqheight);
+static int config_sensor_410(struct camera_data *cam,
+			    int reqwidth, int reqheight);
+static int config_sensor_500(struct camera_data *cam,
+			    int reqwidth, int reqheight);
+static int set_all_properties(struct camera_data *cam);
+static void get_color_params(struct camera_data *cam);
+static void wake_system(struct camera_data *cam);
+static void set_lowlight_boost(struct camera_data *cam);
+static void reset_camera_struct(struct camera_data *cam);
+static int cpia2_set_high_power(struct camera_data *cam);
+
+/* Here we want the physical address of the memory.
+ * This is used when initializing the contents of the
+ * area and marking the pages as reserved.
+ */
+static inline unsigned long kvirt_to_pa(unsigned long adr)
+{
+	unsigned long kva, ret;
+
+	kva = (unsigned long) page_address(vmalloc_to_page((void *)adr));
+	kva |= adr & (PAGE_SIZE-1); /* restore the offset */
+	ret = __pa(kva);
+	return ret;
+}
+
+static void *rvmalloc(unsigned long size)
+{
+	void *mem;
+	unsigned long adr;
+
+	/* Round it off to PAGE_SIZE */
+	size = PAGE_ALIGN(size);
+
+	mem = vmalloc_32(size);
+	if (!mem)
+		return NULL;
+
+	memset(mem, 0, size);	/* Clear the ram out, no junk to the user */
+	adr = (unsigned long) mem;
+
+	while ((long)size > 0) {
+		SetPageReserved(vmalloc_to_page((void *)adr));
+		adr += PAGE_SIZE;
+		size -= PAGE_SIZE;
+	}
+	return mem;
+}
+
+static void rvfree(void *mem, unsigned long size)
+{
+	unsigned long adr;
+
+	if (!mem)
+		return;
+
+	size = PAGE_ALIGN(size);
+
+	adr = (unsigned long) mem;
+	while ((long)size > 0) {
+		ClearPageReserved(vmalloc_to_page((void *)adr));
+		adr += PAGE_SIZE;
+		size -= PAGE_SIZE;
+	}
+	vfree(mem);
+}
+
+/******************************************************************************
+ *
+ *  cpia2_do_command
+ *
+ *  Send an arbitrary command to the camera.  For commands that read from
+ *  the camera, copy the buffers into the proper param structures.
+ *****************************************************************************/
+int cpia2_do_command(struct camera_data *cam,
+		     u32 command, u8 direction, u8 param)
+{
+	int retval = 0;
+	struct cpia2_command cmd;
+	unsigned int device = cam->params.pnp_id.device_type;
+
+	cmd.command = command;
+	cmd.reg_count = 2;	/* default */
+	cmd.direction = direction;
+
+	/***
+	 * Set up the command.
+	 ***/
+	switch (command) {
+	case CPIA2_CMD_GET_VERSION:
+		cmd.req_mode =
+		    CAMERAACCESS_TYPE_BLOCK | CAMERAACCESS_SYSTEM;
+		cmd.start = CPIA2_SYSTEM_DEVICE_HI;
+		break;
+	case CPIA2_CMD_GET_PNP_ID:
+		cmd.req_mode =
+		    CAMERAACCESS_TYPE_BLOCK | CAMERAACCESS_SYSTEM;
+		cmd.reg_count = 8;
+		cmd.start = CPIA2_SYSTEM_DESCRIP_VID_HI;
+		break;
+	case CPIA2_CMD_GET_ASIC_TYPE:
+		cmd.req_mode = CAMERAACCESS_TYPE_BLOCK | CAMERAACCESS_VC;
+		cmd.start = CPIA2_VC_ASIC_ID;
+		break;
+	case CPIA2_CMD_GET_SENSOR:
+		cmd.req_mode = CAMERAACCESS_TYPE_BLOCK | CAMERAACCESS_VP;
+		cmd.start = CPIA2_VP_SENSOR_FLAGS;
+		break;
+	case CPIA2_CMD_GET_VP_DEVICE:
+		cmd.req_mode = CAMERAACCESS_TYPE_BLOCK | CAMERAACCESS_VP;
+		cmd.start = CPIA2_VP_DEVICEH;
+		break;
+	case CPIA2_CMD_SET_VP_BRIGHTNESS:
+		cmd.buffer.block_data[0] = param;	/* Then fall through */
+	case CPIA2_CMD_GET_VP_BRIGHTNESS:
+		cmd.req_mode = CAMERAACCESS_TYPE_BLOCK | CAMERAACCESS_VP;
+		cmd.reg_count = 1;
+		if (device == DEVICE_STV_672)
+			cmd.start = CPIA2_VP4_EXPOSURE_TARGET;
+		else
+			cmd.start = CPIA2_VP5_EXPOSURE_TARGET;
+		break;
+	case CPIA2_CMD_SET_CONTRAST:
+		cmd.buffer.block_data[0] = param;	/* Then fall through */
+	case CPIA2_CMD_GET_CONTRAST:
+		cmd.req_mode = CAMERAACCESS_TYPE_BLOCK | CAMERAACCESS_VP;
+		cmd.reg_count = 1;
+		cmd.start = CPIA2_VP_YRANGE;
+		break;
+	case CPIA2_CMD_SET_VP_SATURATION:
+		cmd.buffer.block_data[0] = param;	/* Then fall through */
+	case CPIA2_CMD_GET_VP_SATURATION:
+		cmd.req_mode = CAMERAACCESS_TYPE_BLOCK | CAMERAACCESS_VP;
+		cmd.reg_count = 1;
+		if (device == DEVICE_STV_672)
+			cmd.start = CPIA2_VP_SATURATION;
+		else
+			cmd.start = CPIA2_VP5_MCUVSATURATION;
+		break;
+	case CPIA2_CMD_SET_VP_GPIO_DATA:
+		cmd.buffer.block_data[0] = param;	/* Then fall through */
+	case CPIA2_CMD_GET_VP_GPIO_DATA:
+		cmd.req_mode = CAMERAACCESS_TYPE_BLOCK | CAMERAACCESS_VP;
+		cmd.reg_count = 1;
+		cmd.start = CPIA2_VP_GPIO_DATA;
+		break;
+	case CPIA2_CMD_SET_VP_GPIO_DIRECTION:
+		cmd.buffer.block_data[0] = param;	/* Then fall through */
+	case CPIA2_CMD_GET_VP_GPIO_DIRECTION:
+		cmd.req_mode = CAMERAACCESS_TYPE_BLOCK | CAMERAACCESS_VP;
+		cmd.reg_count = 1;
+		cmd.start = CPIA2_VP_GPIO_DIRECTION;
+		break;
+	case CPIA2_CMD_SET_VC_MP_GPIO_DATA:
+		cmd.buffer.block_data[0] = param;	/* Then fall through */
+	case CPIA2_CMD_GET_VC_MP_GPIO_DATA:
+		cmd.req_mode = CAMERAACCESS_TYPE_BLOCK | CAMERAACCESS_VC;
+		cmd.reg_count = 1;
+		cmd.start = CPIA2_VC_MP_DATA;
+		break;
+	case CPIA2_CMD_SET_VC_MP_GPIO_DIRECTION:
+		cmd.buffer.block_data[0] = param;	/* Then fall through */
+	case CPIA2_CMD_GET_VC_MP_GPIO_DIRECTION:
+		cmd.req_mode = CAMERAACCESS_TYPE_BLOCK | CAMERAACCESS_VC;
+		cmd.reg_count = 1;
+		cmd.start = CPIA2_VC_MP_DIR;
+		break;
+	case CPIA2_CMD_ENABLE_PACKET_CTRL:
+		cmd.req_mode =
+		    CAMERAACCESS_TYPE_BLOCK | CAMERAACCESS_SYSTEM;
+		cmd.start = CPIA2_SYSTEM_INT_PACKET_CTRL;
+		cmd.reg_count = 1;
+		cmd.buffer.block_data[0] = param;
+		break;
+	case CPIA2_CMD_SET_FLICKER_MODES:
+		cmd.buffer.block_data[0] = param;	/* Then fall through */
+	case CPIA2_CMD_GET_FLICKER_MODES:
+		cmd.req_mode = CAMERAACCESS_TYPE_BLOCK | CAMERAACCESS_VP;
+		cmd.reg_count = 1;
+		cmd.start = CPIA2_VP_FLICKER_MODES;
+		break;
+	case CPIA2_CMD_RESET_FIFO:	/* clear fifo and enable stream block */
+		cmd.req_mode = CAMERAACCESS_TYPE_RANDOM | CAMERAACCESS_VC;
+		cmd.reg_count = 2;
+		cmd.start = 0;
+		cmd.buffer.registers[0].index = CPIA2_VC_ST_CTRL;
+		cmd.buffer.registers[0].value = CPIA2_VC_ST_CTRL_SRC_VC |
+		    CPIA2_VC_ST_CTRL_DST_USB | CPIA2_VC_ST_CTRL_EOF_DETECT;
+		cmd.buffer.registers[1].index = CPIA2_VC_ST_CTRL;
+		cmd.buffer.registers[1].value = CPIA2_VC_ST_CTRL_SRC_VC |
+		    CPIA2_VC_ST_CTRL_DST_USB |
+		    CPIA2_VC_ST_CTRL_EOF_DETECT |
+		    CPIA2_VC_ST_CTRL_FIFO_ENABLE;
+		break;
+	case CPIA2_CMD_SET_HI_POWER:
+		cmd.req_mode =
+		    CAMERAACCESS_TYPE_RANDOM | CAMERAACCESS_SYSTEM;
+		cmd.reg_count = 2;
+		cmd.buffer.registers[0].index =
+		    CPIA2_SYSTEM_SYSTEM_CONTROL;
+		cmd.buffer.registers[1].index =
+		    CPIA2_SYSTEM_SYSTEM_CONTROL;
+		cmd.buffer.registers[0].value = CPIA2_SYSTEM_CONTROL_CLEAR_ERR;
+		cmd.buffer.registers[1].value =
+		    CPIA2_SYSTEM_CONTROL_HIGH_POWER;
+		break;
+	case CPIA2_CMD_SET_LOW_POWER:
+		cmd.req_mode =
+		    CAMERAACCESS_TYPE_BLOCK | CAMERAACCESS_SYSTEM;
+		cmd.reg_count = 1;
+		cmd.start = CPIA2_SYSTEM_SYSTEM_CONTROL;
+		cmd.buffer.block_data[0] = 0;
+		break;
+	case CPIA2_CMD_CLEAR_V2W_ERR:
+		cmd.req_mode =
+		    CAMERAACCESS_TYPE_BLOCK | CAMERAACCESS_SYSTEM;
+		cmd.reg_count = 1;
+		cmd.start = CPIA2_SYSTEM_SYSTEM_CONTROL;
+		cmd.buffer.block_data[0] = CPIA2_SYSTEM_CONTROL_CLEAR_ERR;
+		break;
+	case CPIA2_CMD_SET_USER_MODE:   /* Then fall through */
+		cmd.buffer.block_data[0] = param;
+	case CPIA2_CMD_GET_USER_MODE:
+		cmd.req_mode = CAMERAACCESS_TYPE_BLOCK | CAMERAACCESS_VP;
+		cmd.reg_count = 1;
+		if (device == DEVICE_STV_672)
+			cmd.start = CPIA2_VP4_USER_MODE;
+		else
+			cmd.start = CPIA2_VP5_USER_MODE;
+		break;
+	case CPIA2_CMD_FRAMERATE_REQ:
+		cmd.req_mode = CAMERAACCESS_TYPE_BLOCK | CAMERAACCESS_VP;
+		cmd.reg_count = 1;
+		if (device == DEVICE_STV_672)
+			cmd.start = CPIA2_VP4_FRAMERATE_REQUEST;
+		else
+			cmd.start = CPIA2_VP5_FRAMERATE_REQUEST;
+		cmd.buffer.block_data[0] = param;
+		break;
+	case CPIA2_CMD_SET_WAKEUP:
+		cmd.buffer.block_data[0] = param;	/* Then fall through */
+	case CPIA2_CMD_GET_WAKEUP:
+		cmd.req_mode = CAMERAACCESS_TYPE_BLOCK | CAMERAACCESS_VC;
+		cmd.reg_count = 1;
+		cmd.start = CPIA2_VC_WAKEUP;
+		break;
+	case CPIA2_CMD_SET_PW_CONTROL:
+		cmd.buffer.block_data[0] = param;	/* Then fall through */
+	case CPIA2_CMD_GET_PW_CONTROL:
+		cmd.req_mode = CAMERAACCESS_TYPE_BLOCK | CAMERAACCESS_VC;
+		cmd.reg_count = 1;
+		cmd.start = CPIA2_VC_PW_CTRL;
+		break;
+	case CPIA2_CMD_GET_VP_SYSTEM_STATE:
+		cmd.req_mode = CAMERAACCESS_TYPE_BLOCK | CAMERAACCESS_VP;
+		cmd.reg_count = 1;
+		cmd.start = CPIA2_VP_SYSTEMSTATE;
+		break;
+	case CPIA2_CMD_SET_SYSTEM_CTRL:
+		cmd.buffer.block_data[0] = param;	/* Then fall through */
+	case CPIA2_CMD_GET_SYSTEM_CTRL:
+		cmd.req_mode =
+		    CAMERAACCESS_TYPE_BLOCK | CAMERAACCESS_SYSTEM;
+		cmd.reg_count = 1;
+		cmd.start = CPIA2_SYSTEM_SYSTEM_CONTROL;
+		break;
+	case CPIA2_CMD_SET_VP_SYSTEM_CTRL:
+		cmd.buffer.block_data[0] = param;	/* Then fall through */
+	case CPIA2_CMD_GET_VP_SYSTEM_CTRL:
+		cmd.req_mode = CAMERAACCESS_TYPE_BLOCK | CAMERAACCESS_VP;
+		cmd.reg_count = 1;
+		cmd.start = CPIA2_VP_SYSTEMCTRL;
+		break;
+	case CPIA2_CMD_SET_VP_EXP_MODES:
+		cmd.buffer.block_data[0] = param;	/* Then fall through */
+	case CPIA2_CMD_GET_VP_EXP_MODES:
+		cmd.req_mode = CAMERAACCESS_TYPE_BLOCK | CAMERAACCESS_VP;
+		cmd.reg_count = 1;
+		cmd.start = CPIA2_VP_EXPOSURE_MODES;
+		break;
+	case CPIA2_CMD_SET_DEVICE_CONFIG:
+		cmd.buffer.block_data[0] = param;	/* Then fall through */
+	case CPIA2_CMD_GET_DEVICE_CONFIG:
+		cmd.req_mode = CAMERAACCESS_TYPE_BLOCK | CAMERAACCESS_VP;
+		cmd.reg_count = 1;
+		cmd.start = CPIA2_VP_DEVICE_CONFIG;
+		break;
+	case CPIA2_CMD_SET_SERIAL_ADDR:
+		cmd.buffer.block_data[0] = param;
+		cmd.req_mode =
+		    CAMERAACCESS_TYPE_BLOCK | CAMERAACCESS_SYSTEM;
+		cmd.reg_count = 1;
+		cmd.start = CPIA2_SYSTEM_VP_SERIAL_ADDR;
+		break;
+	case CPIA2_CMD_SET_SENSOR_CR1:
+		cmd.buffer.block_data[0] = param;
+		cmd.req_mode = CAMERAACCESS_TYPE_BLOCK | CAMERAACCESS_VP;
+		cmd.reg_count = 1;
+		cmd.start = CPIA2_SENSOR_CR1;
+		break;
+	case CPIA2_CMD_SET_VC_CONTROL:
+		cmd.buffer.block_data[0] = param;	/* Then fall through */
+	case CPIA2_CMD_GET_VC_CONTROL:
+		cmd.req_mode = CAMERAACCESS_TYPE_BLOCK | CAMERAACCESS_VC;
+		cmd.reg_count = 1;
+		cmd.start = CPIA2_VC_VC_CTRL;
+		break;
+	case CPIA2_CMD_SET_TARGET_KB:
+		cmd.req_mode = CAMERAACCESS_TYPE_RANDOM | CAMERAACCESS_VC;
+		cmd.reg_count = 1;
+		cmd.buffer.registers[0].index = CPIA2_VC_VC_TARGET_KB;
+		cmd.buffer.registers[0].value = param;
+		break;
+	case CPIA2_CMD_SET_DEF_JPEG_OPT:
+		cmd.req_mode = CAMERAACCESS_TYPE_RANDOM | CAMERAACCESS_VC;
+		cmd.reg_count = 4;
+		cmd.buffer.registers[0].index = CPIA2_VC_VC_JPEG_OPT;
+		cmd.buffer.registers[0].value =
+		    CPIA2_VC_VC_JPEG_OPT_DOUBLE_SQUEEZE;
+		cmd.buffer.registers[1].index = CPIA2_VC_VC_USER_SQUEEZE;
+		cmd.buffer.registers[1].value = 20;
+		cmd.buffer.registers[2].index = CPIA2_VC_VC_CREEP_PERIOD;
+		cmd.buffer.registers[2].value = 2;
+		cmd.buffer.registers[3].index = CPIA2_VC_VC_JPEG_OPT;
+		cmd.buffer.registers[3].value = CPIA2_VC_VC_JPEG_OPT_DEFAULT;
+		break;
+	case CPIA2_CMD_REHASH_VP4:
+		cmd.req_mode = CAMERAACCESS_TYPE_BLOCK | CAMERAACCESS_VP;
+		cmd.reg_count = 1;
+		cmd.start = CPIA2_VP_REHASH_VALUES;
+		cmd.buffer.block_data[0] = param;
+		break;
+	case CPIA2_CMD_SET_USER_EFFECTS:  /* Note: Be careful with this as
+					     this register can also affect
+					     flicker modes */
+		cmd.buffer.block_data[0] = param;      /* Then fall through */
+	case CPIA2_CMD_GET_USER_EFFECTS:
+		cmd.req_mode = CAMERAACCESS_TYPE_BLOCK | CAMERAACCESS_VP;
+		cmd.reg_count = 1;
+		if (device == DEVICE_STV_672)
+			cmd.start = CPIA2_VP4_USER_EFFECTS;
+		else
+			cmd.start = CPIA2_VP5_USER_EFFECTS;
+		break;
+	default:
+		LOG("DoCommand received invalid command\n");
+		return -EINVAL;
+	}
+
+	retval = cpia2_send_command(cam, &cmd);
+	if (retval) {
+		return retval;
+	}
+
+	/***
+	 * Now copy any results from a read into the appropriate param struct.
+	 ***/
+	switch (command) {
+	case CPIA2_CMD_GET_VERSION:
+		cam->params.version.firmware_revision_hi =
+		    cmd.buffer.block_data[0];
+		cam->params.version.firmware_revision_lo =
+		    cmd.buffer.block_data[1];
+		break;
+	case CPIA2_CMD_GET_PNP_ID:
+		cam->params.pnp_id.vendor = (cmd.buffer.block_data[0] << 8) |
+					    cmd.buffer.block_data[1];
+		cam->params.pnp_id.product = (cmd.buffer.block_data[2] << 8) |
+					     cmd.buffer.block_data[3];
+		cam->params.pnp_id.device_revision =
+			(cmd.buffer.block_data[4] << 8) |
+			cmd.buffer.block_data[5];
+		if (cam->params.pnp_id.vendor == 0x553) {
+			if (cam->params.pnp_id.product == 0x100) {
+				cam->params.pnp_id.device_type = DEVICE_STV_672;
+			} else if (cam->params.pnp_id.product == 0x140 ||
+				   cam->params.pnp_id.product == 0x151) {
+				cam->params.pnp_id.device_type = DEVICE_STV_676;
+			}
+		}
+		break;
+	case CPIA2_CMD_GET_ASIC_TYPE:
+		cam->params.version.asic_id = cmd.buffer.block_data[0];
+		cam->params.version.asic_rev = cmd.buffer.block_data[1];
+		break;
+	case CPIA2_CMD_GET_SENSOR:
+		cam->params.version.sensor_flags = cmd.buffer.block_data[0];
+		cam->params.version.sensor_rev = cmd.buffer.block_data[1];
+		break;
+	case CPIA2_CMD_GET_VP_DEVICE:
+		cam->params.version.vp_device_hi = cmd.buffer.block_data[0];
+		cam->params.version.vp_device_lo = cmd.buffer.block_data[1];
+		break;
+	case CPIA2_CMD_GET_VP_BRIGHTNESS:
+		cam->params.color_params.brightness = cmd.buffer.block_data[0];
+		break;
+	case CPIA2_CMD_GET_CONTRAST:
+		cam->params.color_params.contrast = cmd.buffer.block_data[0];
+		break;
+	case CPIA2_CMD_GET_VP_SATURATION:
+		cam->params.color_params.saturation = cmd.buffer.block_data[0];
+		break;
+	case CPIA2_CMD_GET_VP_GPIO_DATA:
+		cam->params.vp_params.gpio_data = cmd.buffer.block_data[0];
+		break;
+	case CPIA2_CMD_GET_VP_GPIO_DIRECTION:
+		cam->params.vp_params.gpio_direction = cmd.buffer.block_data[0];
+		break;
+	case CPIA2_CMD_GET_VC_MP_GPIO_DIRECTION:
+		cam->params.vc_params.vc_mp_direction =cmd.buffer.block_data[0];
+		break;
+	case CPIA2_CMD_GET_VC_MP_GPIO_DATA:
+		cam->params.vc_params.vc_mp_data = cmd.buffer.block_data[0];
+		break;
+	case CPIA2_CMD_GET_FLICKER_MODES:
+		cam->params.flicker_control.cam_register =
+			cmd.buffer.block_data[0];
+		break;
+	case CPIA2_CMD_GET_WAKEUP:
+		cam->params.vc_params.wakeup = cmd.buffer.block_data[0];
+		break;
+	case CPIA2_CMD_GET_PW_CONTROL:
+		cam->params.vc_params.pw_control = cmd.buffer.block_data[0];
+		break;
+	case CPIA2_CMD_GET_SYSTEM_CTRL:
+		cam->params.camera_state.system_ctrl = cmd.buffer.block_data[0];
+		break;
+	case CPIA2_CMD_GET_VP_SYSTEM_STATE:
+		cam->params.vp_params.system_state = cmd.buffer.block_data[0];
+		break;
+	case CPIA2_CMD_GET_VP_SYSTEM_CTRL:
+		cam->params.vp_params.system_ctrl = cmd.buffer.block_data[0];
+		break;
+	case CPIA2_CMD_GET_VP_EXP_MODES:
+		cam->params.vp_params.exposure_modes = cmd.buffer.block_data[0];
+		break;
+	case CPIA2_CMD_GET_DEVICE_CONFIG:
+		cam->params.vp_params.device_config = cmd.buffer.block_data[0];
+		break;
+	case CPIA2_CMD_GET_VC_CONTROL:
+		cam->params.vc_params.vc_control = cmd.buffer.block_data[0];
+		break;
+	case CPIA2_CMD_GET_USER_MODE:
+		cam->params.vp_params.video_mode = cmd.buffer.block_data[0];
+		break;
+	case CPIA2_CMD_GET_USER_EFFECTS:
+		cam->params.vp_params.user_effects = cmd.buffer.block_data[0];
+		break;
+	default:
+		break;
+	}
+	return retval;
+}
+
+/******************************************************************************
+ *
+ *  cpia2_send_command
+ *
+ *****************************************************************************/
+int cpia2_send_command(struct camera_data *cam, struct cpia2_command *cmd)
+{
+	u8 count;
+	u8 start;
+	u8 block_index;
+	u8 *buffer;
+	int retval;
+	const char* dir;
+
+	if (cmd->direction == TRANSFER_WRITE) {
+		dir = "Write";
+	} else {
+		dir = "Read";
+	}
+
+	block_index = cmd->req_mode & 0x03;
+
+	switch (cmd->req_mode & 0x0c) {
+	case CAMERAACCESS_TYPE_RANDOM:
+		count = cmd->reg_count * sizeof(struct cpia2_register);
+		start = 0;
+		buffer = (u8 *) & cmd->buffer;
+		if (debugs_on & DEBUG_REG)
+			DBG("%s Random: Register block %s\n", dir,
+			    block_name[block_index]);
+		break;
+	case CAMERAACCESS_TYPE_BLOCK:
+		count = cmd->reg_count;
+		start = cmd->start;
+		buffer = cmd->buffer.block_data;
+		if (debugs_on & DEBUG_REG)
+			DBG("%s Block: Register block %s\n", dir,
+			    block_name[block_index]);
+		break;
+	case CAMERAACCESS_TYPE_MASK:
+		count = cmd->reg_count * sizeof(struct cpia2_reg_mask);
+		start = 0;
+		buffer = (u8 *) & cmd->buffer;
+		if (debugs_on & DEBUG_REG)
+			DBG("%s Mask: Register block %s\n", dir,
+			    block_name[block_index]);
+		break;
+	case CAMERAACCESS_TYPE_REPEAT:	/* For patch blocks only */
+		count = cmd->reg_count;
+		start = cmd->start;
+		buffer = cmd->buffer.block_data;
+		if (debugs_on & DEBUG_REG)
+			DBG("%s Repeat: Register block %s\n", dir,
+			    block_name[block_index]);
+		break;
+	default:
+		LOG("%s: invalid request mode\n",__FUNCTION__);
+		return -EINVAL;
+	}
+
+	retval = cpia2_usb_transfer_cmd(cam,
+					buffer,
+					cmd->req_mode,
+					start, count, cmd->direction);
+#ifdef _CPIA2_DEBUG_
+	if (debugs_on & DEBUG_REG) {
+		int i;
+		for (i = 0; i < cmd->reg_count; i++) {
+			if((cmd->req_mode & 0x0c) == CAMERAACCESS_TYPE_BLOCK)
+				KINFO("%s Block: [0x%02X] = 0x%02X\n",
+				    dir, start + i, buffer[i]);
+			if((cmd->req_mode & 0x0c) == CAMERAACCESS_TYPE_RANDOM)
+				KINFO("%s Random: [0x%02X] = 0x%02X\n",
+				    dir, cmd->buffer.registers[i].index,
+				    cmd->buffer.registers[i].value);
+		}
+	}
+#endif
+
+	return retval;
+};
+
+/*************
+ * Functions to implement camera functionality
+ *************/
+/******************************************************************************
+ *
+ *  cpia2_get_version_info
+ *
+ *****************************************************************************/
+static void cpia2_get_version_info(struct camera_data *cam)
+{
+	cpia2_do_command(cam, CPIA2_CMD_GET_VERSION, TRANSFER_READ, 0);
+	cpia2_do_command(cam, CPIA2_CMD_GET_PNP_ID, TRANSFER_READ, 0);
+	cpia2_do_command(cam, CPIA2_CMD_GET_ASIC_TYPE, TRANSFER_READ, 0);
+	cpia2_do_command(cam, CPIA2_CMD_GET_SENSOR, TRANSFER_READ, 0);
+	cpia2_do_command(cam, CPIA2_CMD_GET_VP_DEVICE, TRANSFER_READ, 0);
+}
+
+/******************************************************************************
+ *
+ *  cpia2_reset_camera
+ *
+ *  Called at least during the open process, sets up initial params.
+ *****************************************************************************/
+int cpia2_reset_camera(struct camera_data *cam)
+{
+	u8 tmp_reg;
+	int retval = 0;
+	int i;
+	struct cpia2_command cmd;
+
+	/***
+	 * VC setup
+	 ***/
+	retval = configure_sensor(cam,
+				  cam->params.roi.width,
+				  cam->params.roi.height);
+	if (retval < 0) {
+		ERR("Couldn't configure sensor, error=%d\n", retval);
+		return retval;
+	}
+
+	/* Clear FIFO and route/enable stream block */
+	cmd.req_mode = CAMERAACCESS_TYPE_RANDOM | CAMERAACCESS_VC;
+	cmd.direction = TRANSFER_WRITE;
+	cmd.reg_count = 2;
+	cmd.buffer.registers[0].index = CPIA2_VC_ST_CTRL;
+	cmd.buffer.registers[0].value = CPIA2_VC_ST_CTRL_SRC_VC |
+		CPIA2_VC_ST_CTRL_DST_USB | CPIA2_VC_ST_CTRL_EOF_DETECT;
+	cmd.buffer.registers[1].index = CPIA2_VC_ST_CTRL;
+	cmd.buffer.registers[1].value = CPIA2_VC_ST_CTRL_SRC_VC |
+		CPIA2_VC_ST_CTRL_DST_USB |
+		CPIA2_VC_ST_CTRL_EOF_DETECT | CPIA2_VC_ST_CTRL_FIFO_ENABLE;
+
+	cpia2_send_command(cam, &cmd);
+
+	cpia2_set_high_power(cam);
+
+	if (cam->params.pnp_id.device_type == DEVICE_STV_672) {
+		/* Enable button notification */
+		cmd.req_mode = CAMERAACCESS_TYPE_RANDOM | CAMERAACCESS_SYSTEM;
+		cmd.buffer.registers[0].index = CPIA2_SYSTEM_INT_PACKET_CTRL;
+		cmd.buffer.registers[0].value =
+			CPIA2_SYSTEM_INT_PACKET_CTRL_ENABLE_SW_XX;
+		cmd.reg_count = 1;
+		cpia2_send_command(cam, &cmd);
+	}
+
+	current->state = TASK_INTERRUPTIBLE;
+	schedule_timeout(100 * HZ / 1000);	/* wait for 100 msecs */
+
+	if (cam->params.pnp_id.device_type == DEVICE_STV_672)
+		retval = apply_vp_patch(cam);
+
+	/* wait for vp to go to sleep */
+	current->state = TASK_INTERRUPTIBLE;
+	schedule_timeout(100 * HZ / 1000);	/* wait for 100 msecs */
+
+	/***
+	 * If this is a 676, apply VP5 fixes before we start streaming
+	 ***/
+	if (cam->params.pnp_id.device_type == DEVICE_STV_676) {
+		cmd.req_mode = CAMERAACCESS_TYPE_RANDOM | CAMERAACCESS_VP;
+
+		/* The following writes improve the picture */
+		cmd.buffer.registers[0].index = CPIA2_VP5_MYBLACK_LEVEL;
+		cmd.buffer.registers[0].value = 0; /* reduce from the default
+						    * rec 601 pedestal of 16 */
+		cmd.buffer.registers[1].index = CPIA2_VP5_MCYRANGE;
+		cmd.buffer.registers[1].value = 0x92; /* increase from 100% to
+						       * (256/256 - 31) to fill
+						       * available range */
+		cmd.buffer.registers[2].index = CPIA2_VP5_MYCEILING;
+		cmd.buffer.registers[2].value = 0xFF; /* Increase from the
+						       * default rec 601 ceiling
+						       * of 240 */
+		cmd.buffer.registers[3].index = CPIA2_VP5_MCUVSATURATION;
+		cmd.buffer.registers[3].value = 0xFF; /* Increase from the rec
+						       * 601 100% level (128)
+						       * to 145-192 */
+		cmd.buffer.registers[4].index = CPIA2_VP5_ANTIFLKRSETUP;
+		cmd.buffer.registers[4].value = 0x80;  /* Inhibit the
+							* anti-flicker */
+
+		/* The following 4 writes are a fix to allow QVGA to work at 30 fps */
+		cmd.buffer.registers[5].index = CPIA2_VP_RAM_ADDR_H;
+		cmd.buffer.registers[5].value = 0x01;
+		cmd.buffer.registers[6].index = CPIA2_VP_RAM_ADDR_L;
+		cmd.buffer.registers[6].value = 0xE3;
+		cmd.buffer.registers[7].index = CPIA2_VP_RAM_DATA;
+		cmd.buffer.registers[7].value = 0x02;
+		cmd.buffer.registers[8].index = CPIA2_VP_RAM_DATA;
+		cmd.buffer.registers[8].value = 0xFC;
+
+		cmd.direction = TRANSFER_WRITE;
+		cmd.reg_count = 9;
+
+		cpia2_send_command(cam, &cmd);
+	}
+
+	/* Activate all settings and start the data stream */
+	/* Set user mode */
+	set_default_user_mode(cam);
+
+	/* Give VP time to wake up */
+	current->state = TASK_INTERRUPTIBLE;
+	schedule_timeout(100 * HZ / 1000);	/* wait for 100 msecs */
+
+	set_all_properties(cam);
+
+	cpia2_do_command(cam, CPIA2_CMD_GET_USER_MODE, TRANSFER_READ, 0);
+	DBG("After SetAllProperties(cam), user mode is 0x%0X\n",
+	    cam->params.vp_params.video_mode);
+
+	/***
+	 * Set audio regulator off.  This and the code to set the compresison
+	 * state are too complex to form a CPIA2_CMD_, and seem to be somewhat
+	 * intertwined.  This stuff came straight from the windows driver.
+	 ***/
+	/* Turn AutoExposure off in VP and enable the serial bridge to the sensor */
+	cpia2_do_command(cam, CPIA2_CMD_GET_VP_SYSTEM_CTRL, TRANSFER_READ, 0);
+	tmp_reg = cam->params.vp_params.system_ctrl;
+	cmd.buffer.registers[0].value = tmp_reg &
+		(tmp_reg & (CPIA2_VP_SYSTEMCTRL_HK_CONTROL ^ 0xFF));
+
+	cpia2_do_command(cam, CPIA2_CMD_GET_DEVICE_CONFIG, TRANSFER_READ, 0);
+	cmd.buffer.registers[1].value = cam->params.vp_params.device_config |
+					CPIA2_VP_DEVICE_CONFIG_SERIAL_BRIDGE;
+	cmd.buffer.registers[0].index = CPIA2_VP_SYSTEMCTRL;
+	cmd.buffer.registers[1].index = CPIA2_VP_DEVICE_CONFIG;
+	cmd.req_mode = CAMERAACCESS_TYPE_RANDOM | CAMERAACCESS_VP;
+	cmd.reg_count = 2;
+	cmd.direction = TRANSFER_WRITE;
+	cmd.start = 0;
+	cpia2_send_command(cam, &cmd);
+
+	/* Set the correct I2C address in the CPiA-2 system register */
+	cpia2_do_command(cam,
+			 CPIA2_CMD_SET_SERIAL_ADDR,
+			 TRANSFER_WRITE,
+			 CPIA2_SYSTEM_VP_SERIAL_ADDR_SENSOR);
+
+	/* Now have sensor access - set bit to turn the audio regulator off */
+	cpia2_do_command(cam,
+			 CPIA2_CMD_SET_SENSOR_CR1,
+			 TRANSFER_WRITE, CPIA2_SENSOR_CR1_DOWN_AUDIO_REGULATOR);
+
+	/* Set the correct I2C address in the CPiA-2 system register */
+	if (cam->params.pnp_id.device_type == DEVICE_STV_672)
+		cpia2_do_command(cam,
+				 CPIA2_CMD_SET_SERIAL_ADDR,
+				 TRANSFER_WRITE,
+				 CPIA2_SYSTEM_VP_SERIAL_ADDR_VP); // 0x88
+	else
+		cpia2_do_command(cam,
+				 CPIA2_CMD_SET_SERIAL_ADDR,
+				 TRANSFER_WRITE,
+				 CPIA2_SYSTEM_VP_SERIAL_ADDR_676_VP); // 0x8a
+
+	/* increase signal drive strength */
+	if (cam->params.pnp_id.device_type == DEVICE_STV_676)
+		cpia2_do_command(cam,
+				 CPIA2_CMD_SET_VP_EXP_MODES,
+				 TRANSFER_WRITE,
+				 CPIA2_VP_EXPOSURE_MODES_COMPILE_EXP);
+
+	/* Start autoexposure */
+	cpia2_do_command(cam, CPIA2_CMD_GET_DEVICE_CONFIG, TRANSFER_READ, 0);
+	cmd.buffer.registers[0].value = cam->params.vp_params.device_config &
+				  (CPIA2_VP_DEVICE_CONFIG_SERIAL_BRIDGE ^ 0xFF);
+
+	cpia2_do_command(cam, CPIA2_CMD_GET_VP_SYSTEM_CTRL, TRANSFER_READ, 0);
+	cmd.buffer.registers[1].value =
+	    cam->params.vp_params.system_ctrl | CPIA2_VP_SYSTEMCTRL_HK_CONTROL;
+
+	cmd.buffer.registers[0].index = CPIA2_VP_DEVICE_CONFIG;
+	cmd.buffer.registers[1].index = CPIA2_VP_SYSTEMCTRL;
+	cmd.req_mode = CAMERAACCESS_TYPE_RANDOM | CAMERAACCESS_VP;
+	cmd.reg_count = 2;
+	cmd.direction = TRANSFER_WRITE;
+
+	cpia2_send_command(cam, &cmd);
+
+	/* Set compression state */
+	cpia2_do_command(cam, CPIA2_CMD_GET_VC_CONTROL, TRANSFER_READ, 0);
+	if (cam->params.compression.inhibit_htables) {
+		tmp_reg = cam->params.vc_params.vc_control |
+			  CPIA2_VC_VC_CTRL_INHIBIT_H_TABLES;
+	} else  {
+		tmp_reg = cam->params.vc_params.vc_control &
+			  ~CPIA2_VC_VC_CTRL_INHIBIT_H_TABLES;
+	}
+	cpia2_do_command(cam, CPIA2_CMD_SET_VC_CONTROL, TRANSFER_WRITE,tmp_reg);
+
+	/* Set target size (kb) on vc */
+	cpia2_do_command(cam, CPIA2_CMD_SET_TARGET_KB,
+			 TRANSFER_WRITE, cam->params.vc_params.target_kb);
+
+	/* Wiggle VC Reset */
+	/***
+	 * First read and wait a bit.
+	 ***/
+	for (i = 0; i < 50; i++) {
+		cpia2_do_command(cam, CPIA2_CMD_GET_PW_CONTROL,
+				 TRANSFER_READ, 0);
+	}
+
+	tmp_reg = cam->params.vc_params.pw_control;
+	tmp_reg &= ~CPIA2_VC_PW_CTRL_VC_RESET_N;
+
+	cpia2_do_command(cam, CPIA2_CMD_SET_PW_CONTROL, TRANSFER_WRITE,tmp_reg);
+
+	tmp_reg |= CPIA2_VC_PW_CTRL_VC_RESET_N;
+	cpia2_do_command(cam, CPIA2_CMD_SET_PW_CONTROL, TRANSFER_WRITE,tmp_reg);
+
+	cpia2_do_command(cam, CPIA2_CMD_SET_DEF_JPEG_OPT, TRANSFER_WRITE, 0);
+
+	cpia2_do_command(cam, CPIA2_CMD_GET_USER_MODE, TRANSFER_READ, 0);
+	DBG("After VC RESET, user mode is 0x%0X\n",
+	    cam->params.vp_params.video_mode);
+
+	return retval;
+}
+
+/******************************************************************************
+ *
+ *  cpia2_set_high_power
+ *
+ *****************************************************************************/
+static int cpia2_set_high_power(struct camera_data *cam)
+{
+	int i;
+	for (i = 0; i <= 50; i++) {
+		/* Read system status */
+		cpia2_do_command(cam,CPIA2_CMD_GET_SYSTEM_CTRL,TRANSFER_READ,0);
+
+		/* If there is an error, clear it */
+		if(cam->params.camera_state.system_ctrl &
+		   CPIA2_SYSTEM_CONTROL_V2W_ERR)
+			cpia2_do_command(cam, CPIA2_CMD_CLEAR_V2W_ERR,
+					 TRANSFER_WRITE, 0);
+
+		/* Try to set high power mode */
+		cpia2_do_command(cam, CPIA2_CMD_SET_SYSTEM_CTRL,
+				 TRANSFER_WRITE, 1);
+
+		/* Try to read something in VP to check if everything is awake */
+		cpia2_do_command(cam, CPIA2_CMD_GET_VP_SYSTEM_STATE,
+				 TRANSFER_READ, 0);
+		if (cam->params.vp_params.system_state &
+		    CPIA2_VP_SYSTEMSTATE_HK_ALIVE) {
+			break;
+		} else if (i == 50) {
+			cam->params.camera_state.power_mode = LO_POWER_MODE;
+			ERR("Camera did not wake up\n");
+			return -EIO;
+		}
+	}
+
+	DBG("System now in high power state\n");
+	cam->params.camera_state.power_mode = HI_POWER_MODE;
+	return 0;
+}
+
+/******************************************************************************
+ *
+ *  cpia2_set_low_power
+ *
+ *****************************************************************************/
+int cpia2_set_low_power(struct camera_data *cam)
+{
+	cam->params.camera_state.power_mode = LO_POWER_MODE;
+	cpia2_do_command(cam, CPIA2_CMD_SET_SYSTEM_CTRL, TRANSFER_WRITE, 0);
+	return 0;
+}
+
+/******************************************************************************
+ *
+ *  apply_vp_patch
+ *
+ *****************************************************************************/
+static int apply_vp_patch(struct camera_data *cam)
+{
+	int i, j;
+	struct cpia2_command cmd;
+
+	cmd.req_mode = CAMERAACCESS_TYPE_REPEAT | CAMERAACCESS_VP;
+	cmd.direction = TRANSFER_WRITE;
+
+	for (i = 0; i < PATCH_DATA_SIZE; i++) {
+		for (j = 0; j < patch_data[i].count; j++) {
+			cmd.buffer.block_data[j] = patch_data[i].data[j];
+		}
+
+		cmd.start = patch_data[i].reg;
+		cmd.reg_count = patch_data[i].count;
+		cpia2_send_command(cam, &cmd);
+	}
+
+	return 0;
+}
+
+/******************************************************************************
+ *
+ *  set_default_user_mode
+ *
+ *****************************************************************************/
+static int set_default_user_mode(struct camera_data *cam)
+{
+	unsigned char user_mode;
+	unsigned char frame_rate;
+	int width = cam->params.roi.width;
+	int height = cam->params.roi.height;
+
+	switch (cam->params.version.sensor_flags) {
+	case CPIA2_VP_SENSOR_FLAGS_404:
+	case CPIA2_VP_SENSOR_FLAGS_407:
+	case CPIA2_VP_SENSOR_FLAGS_409:
+	case CPIA2_VP_SENSOR_FLAGS_410:
+		if ((width > STV_IMAGE_QCIF_COLS)
+		    || (height > STV_IMAGE_QCIF_ROWS)) {
+			user_mode = CPIA2_VP_USER_MODE_CIF;
+		} else {
+			user_mode = CPIA2_VP_USER_MODE_QCIFDS;
+		}
+		frame_rate = CPIA2_VP_FRAMERATE_30;
+		break;
+	case CPIA2_VP_SENSOR_FLAGS_500:
+		if ((width > STV_IMAGE_CIF_COLS)
+		    || (height > STV_IMAGE_CIF_ROWS)) {
+			user_mode = CPIA2_VP_USER_MODE_VGA;
+		} else {
+			user_mode = CPIA2_VP_USER_MODE_QVGADS;
+		}
+		if (cam->params.pnp_id.device_type == DEVICE_STV_672)
+			frame_rate = CPIA2_VP_FRAMERATE_15;
+		else
+			frame_rate = CPIA2_VP_FRAMERATE_30;
+		break;
+	default:
+		LOG("%s: Invalid sensor flag value 0x%0X\n",__FUNCTION__,
+		    cam->params.version.sensor_flags);
+		return -EINVAL;
+	}
+
+	DBG("Sensor flag = 0x%0x, user mode = 0x%0x, frame rate = 0x%X\n",
+	    cam->params.version.sensor_flags, user_mode, frame_rate);
+	cpia2_do_command(cam, CPIA2_CMD_SET_USER_MODE, TRANSFER_WRITE,
+			 user_mode);
+	if(cam->params.vp_params.frame_rate > 0 &&
+	   frame_rate > cam->params.vp_params.frame_rate)
+		frame_rate = cam->params.vp_params.frame_rate;
+
+	cpia2_set_fps(cam, frame_rate);
+
+//	if (cam->params.pnp_id.device_type == DEVICE_STV_676)
+//		cpia2_do_command(cam,
+//				 CPIA2_CMD_SET_VP_SYSTEM_CTRL,
+//				 TRANSFER_WRITE,
+//				 CPIA2_VP_SYSTEMCTRL_HK_CONTROL |
+//				 CPIA2_VP_SYSTEMCTRL_POWER_CONTROL);
+
+	return 0;
+}
+
+/******************************************************************************
+ *
+ *  cpia2_match_video_size
+ *
+ *  return the best match, where 'best' is as always
+ *  the largest that is not bigger than what is requested.
+ *****************************************************************************/
+int cpia2_match_video_size(int width, int height)
+{
+	if (width >= STV_IMAGE_VGA_COLS && height >= STV_IMAGE_VGA_ROWS)
+		return VIDEOSIZE_VGA;
+
+	if (width >= STV_IMAGE_CIF_COLS && height >= STV_IMAGE_CIF_ROWS)
+		return VIDEOSIZE_CIF;
+
+	if (width >= STV_IMAGE_QVGA_COLS && height >= STV_IMAGE_QVGA_ROWS)
+		return VIDEOSIZE_QVGA;
+
+	if (width >= 288 && height >= 216)
+		return VIDEOSIZE_288_216;
+
+	if (width >= 256 && height >= 192)
+		return VIDEOSIZE_256_192;
+
+	if (width >= 224 && height >= 168)
+		return VIDEOSIZE_224_168;
+
+	if (width >= 192 && height >= 144)
+		return VIDEOSIZE_192_144;
+
+	if (width >= STV_IMAGE_QCIF_COLS && height >= STV_IMAGE_QCIF_ROWS)
+		return VIDEOSIZE_QCIF;
+
+	return -1;
+}
+
+/******************************************************************************
+ *
+ *  SetVideoSize
+ *
+ *****************************************************************************/
+static int set_vw_size(struct camera_data *cam, int size)
+{
+	int retval = 0;
+
+	cam->params.vp_params.video_size = size;
+
+	switch (size) {
+	case VIDEOSIZE_VGA:
+		DBG("Setting size to VGA\n");
+		cam->params.roi.width = STV_IMAGE_VGA_COLS;
+		cam->params.roi.height = STV_IMAGE_VGA_ROWS;
+		cam->vw.width = STV_IMAGE_VGA_COLS;
+		cam->vw.height = STV_IMAGE_VGA_ROWS;
+		break;
+	case VIDEOSIZE_CIF:
+		DBG("Setting size to CIF\n");
+		cam->params.roi.width = STV_IMAGE_CIF_COLS;
+		cam->params.roi.height = STV_IMAGE_CIF_ROWS;
+		cam->vw.width = STV_IMAGE_CIF_COLS;
+		cam->vw.height = STV_IMAGE_CIF_ROWS;
+		break;
+	case VIDEOSIZE_QVGA:
+		DBG("Setting size to QVGA\n");
+		cam->params.roi.width = STV_IMAGE_QVGA_COLS;
+		cam->params.roi.height = STV_IMAGE_QVGA_ROWS;
+		cam->vw.width = STV_IMAGE_QVGA_COLS;
+		cam->vw.height = STV_IMAGE_QVGA_ROWS;
+		break;
+	case VIDEOSIZE_288_216:
+		cam->params.roi.width = 288;
+		cam->params.roi.height = 216;
+		cam->vw.width = 288;
+		cam->vw.height = 216;
+		break;
+	case VIDEOSIZE_256_192:
+		cam->vw.width = 256;
+		cam->vw.height = 192;
+		cam->params.roi.width = 256;
+		cam->params.roi.height = 192;
+		break;
+	case VIDEOSIZE_224_168:
+		cam->vw.width = 224;
+		cam->vw.height = 168;
+		cam->params.roi.width = 224;
+		cam->params.roi.height = 168;
+		break;
+	case VIDEOSIZE_192_144:
+		cam->vw.width = 192;
+		cam->vw.height = 144;
+		cam->params.roi.width = 192;
+		cam->params.roi.height = 144;
+		break;
+	case VIDEOSIZE_QCIF:
+		DBG("Setting size to QCIF\n");
+		cam->params.roi.width = STV_IMAGE_QCIF_COLS;
+		cam->params.roi.height = STV_IMAGE_QCIF_ROWS;
+		cam->vw.width = STV_IMAGE_QCIF_COLS;
+		cam->vw.height = STV_IMAGE_QCIF_ROWS;
+		break;
+	default:
+		retval = -EINVAL;
+	}
+	return retval;
+}
+
+/******************************************************************************
+ *
+ *  configure_sensor
+ *
+ *****************************************************************************/
+static int configure_sensor(struct camera_data *cam,
+			    int req_width, int req_height)
+{
+	int retval;
+
+	switch (cam->params.version.sensor_flags) {
+	case CPIA2_VP_SENSOR_FLAGS_404:
+	case CPIA2_VP_SENSOR_FLAGS_407:
+	case CPIA2_VP_SENSOR_FLAGS_409:
+	case CPIA2_VP_SENSOR_FLAGS_410:
+		retval = config_sensor_410(cam, req_width, req_height);
+		break;
+	case CPIA2_VP_SENSOR_FLAGS_500:
+		retval = config_sensor_500(cam, req_width, req_height);
+		break;
+	default:
+		return -EINVAL;
+	}
+
+	return retval;
+}
+
+/******************************************************************************
+ *
+ *  config_sensor_410
+ *
+ *****************************************************************************/
+static int config_sensor_410(struct camera_data *cam,
+			    int req_width, int req_height)
+{
+	struct cpia2_command cmd;
+	int i = 0;
+	int image_size;
+	int image_type;
+	int width = req_width;
+	int height = req_height;
+
+	/***
+	 *  Make sure size doesn't exceed CIF.
+	 ***/
+	if (width > STV_IMAGE_CIF_COLS)
+		width = STV_IMAGE_CIF_COLS;
+	if (height > STV_IMAGE_CIF_ROWS)
+		height = STV_IMAGE_CIF_ROWS;
+
+	image_size = cpia2_match_video_size(width, height);
+
+	DBG("Config 410: width = %d, height = %d\n", width, height);
+	DBG("Image size returned is %d\n", image_size);
+	if (image_size >= 0) {
+		set_vw_size(cam, image_size);
+		width = cam->params.roi.width;
+		height = cam->params.roi.height;
+
+		DBG("After set_vw_size(), width = %d, height = %d\n",
+		    width, height);
+		if (width <= 176 && height <= 144) {
+			DBG("image type = VIDEOSIZE_QCIF\n");
+			image_type = VIDEOSIZE_QCIF;
+		}
+		else if (width <= 320 && height <= 240) {
+			DBG("image type = VIDEOSIZE_QVGA\n");
+			image_type = VIDEOSIZE_QVGA;
+		}
+		else {
+			DBG("image type = VIDEOSIZE_CIF\n");
+			image_type = VIDEOSIZE_CIF;
+		}
+	} else {
+		ERR("ConfigSensor410 failed\n");
+		return -EINVAL;
+	}
+
+	cmd.req_mode = CAMERAACCESS_TYPE_RANDOM | CAMERAACCESS_VC;
+	cmd.direction = TRANSFER_WRITE;
+
+	/* VC Format */
+	cmd.buffer.registers[i].index = CPIA2_VC_VC_FORMAT;
+	if (image_type == VIDEOSIZE_CIF) {
+		cmd.buffer.registers[i++].value =
+		    (u8) (CPIA2_VC_VC_FORMAT_UFIRST |
+			  CPIA2_VC_VC_FORMAT_SHORTLINE);
+	} else {
+		cmd.buffer.registers[i++].value =
+		    (u8) CPIA2_VC_VC_FORMAT_UFIRST;
+	}
+
+	/* VC Clocks */
+	cmd.buffer.registers[i].index = CPIA2_VC_VC_CLOCKS;
+	if (image_type == VIDEOSIZE_QCIF) {
+		if (cam->params.pnp_id.device_type == DEVICE_STV_672) {
+			cmd.buffer.registers[i++].value=
+				(u8)(CPIA2_VC_VC_672_CLOCKS_CIF_DIV_BY_3 |
+				     CPIA2_VC_VC_672_CLOCKS_SCALING |
+				     CPIA2_VC_VC_CLOCKS_LOGDIV2);
+			DBG("VC_Clocks (0xc4) should be B\n");
+		}
+		else {
+			cmd.buffer.registers[i++].value=
+				(u8)(CPIA2_VC_VC_676_CLOCKS_CIF_DIV_BY_3 |
+				     CPIA2_VC_VC_CLOCKS_LOGDIV2);
+		}
+	} else {
+		if (cam->params.pnp_id.device_type == DEVICE_STV_672) {
+			cmd.buffer.registers[i++].value =
+			   (u8) (CPIA2_VC_VC_672_CLOCKS_CIF_DIV_BY_3 |
+				 CPIA2_VC_VC_CLOCKS_LOGDIV0);
+		}
+		else {
+			cmd.buffer.registers[i++].value =
+			   (u8) (CPIA2_VC_VC_676_CLOCKS_CIF_DIV_BY_3 |
+				 CPIA2_VC_VC_676_CLOCKS_SCALING |
+				 CPIA2_VC_VC_CLOCKS_LOGDIV0);
+		}
+	}
+	DBG("VC_Clocks (0xc4) = 0x%0X\n", cmd.buffer.registers[i-1].value);
+
+	/* Input reqWidth from VC */
+	cmd.buffer.registers[i].index = CPIA2_VC_VC_IHSIZE_LO;
+	if (image_type == VIDEOSIZE_QCIF)
+		cmd.buffer.registers[i++].value =
+		    (u8) (STV_IMAGE_QCIF_COLS / 4);
+	else
+		cmd.buffer.registers[i++].value =
+		    (u8) (STV_IMAGE_CIF_COLS / 4);
+
+	/* Timings */
+	cmd.buffer.registers[i].index = CPIA2_VC_VC_XLIM_HI;
+	if (image_type == VIDEOSIZE_QCIF)
+		cmd.buffer.registers[i++].value = (u8) 0;
+	else
+		cmd.buffer.registers[i++].value = (u8) 1;
+
+	cmd.buffer.registers[i].index = CPIA2_VC_VC_XLIM_LO;
+	if (image_type == VIDEOSIZE_QCIF)
+		cmd.buffer.registers[i++].value = (u8) 208;
+	else
+		cmd.buffer.registers[i++].value = (u8) 160;
+
+	cmd.buffer.registers[i].index = CPIA2_VC_VC_YLIM_HI;
+	if (image_type == VIDEOSIZE_QCIF)
+		cmd.buffer.registers[i++].value = (u8) 0;
+	else
+		cmd.buffer.registers[i++].value = (u8) 1;
+
+	cmd.buffer.registers[i].index = CPIA2_VC_VC_YLIM_LO;
+	if (image_type == VIDEOSIZE_QCIF)
+		cmd.buffer.registers[i++].value = (u8) 160;
+	else
+		cmd.buffer.registers[i++].value = (u8) 64;
+
+	/* Output Image Size */
+	cmd.buffer.registers[i].index = CPIA2_VC_VC_OHSIZE;
+	cmd.buffer.registers[i++].value = cam->params.roi.width / 4;
+
+	cmd.buffer.registers[i].index = CPIA2_VC_VC_OVSIZE;
+	cmd.buffer.registers[i++].value = cam->params.roi.height / 4;
+
+	/* Cropping */
+	cmd.buffer.registers[i].index = CPIA2_VC_VC_HCROP;
+	if (image_type == VIDEOSIZE_QCIF)
+		cmd.buffer.registers[i++].value =
+		    (u8) (((STV_IMAGE_QCIF_COLS / 4) - (width / 4)) / 2);
+	else
+		cmd.buffer.registers[i++].value =
+		    (u8) (((STV_IMAGE_CIF_COLS / 4) - (width / 4)) / 2);
+
+	cmd.buffer.registers[i].index = CPIA2_VC_VC_VCROP;
+	if (image_type == VIDEOSIZE_QCIF)
+		cmd.buffer.registers[i++].value =
+		    (u8) (((STV_IMAGE_QCIF_ROWS / 4) - (height / 4)) / 2);
+	else
+		cmd.buffer.registers[i++].value =
+		    (u8) (((STV_IMAGE_CIF_ROWS / 4) - (height / 4)) / 2);
+
+	/* Scaling registers (defaults) */
+	cmd.buffer.registers[i].index = CPIA2_VC_VC_HPHASE;
+	cmd.buffer.registers[i++].value = (u8) 0;
+
+	cmd.buffer.registers[i].index = CPIA2_VC_VC_VPHASE;
+	cmd.buffer.registers[i++].value = (u8) 0;
+
+	cmd.buffer.registers[i].index = CPIA2_VC_VC_HISPAN;
+	cmd.buffer.registers[i++].value = (u8) 31;
+
+	cmd.buffer.registers[i].index = CPIA2_VC_VC_VISPAN;
+	cmd.buffer.registers[i++].value = (u8) 31;
+
+	cmd.buffer.registers[i].index = CPIA2_VC_VC_HICROP;
+	cmd.buffer.registers[i++].value = (u8) 0;
+
+	cmd.buffer.registers[i].index = CPIA2_VC_VC_VICROP;
+	cmd.buffer.registers[i++].value = (u8) 0;
+
+	cmd.buffer.registers[i].index = CPIA2_VC_VC_HFRACT;
+	cmd.buffer.registers[i++].value = (u8) 0x81;	/* = 8/1 = 8 (HIBYTE/LOBYTE) */
+
+	cmd.buffer.registers[i].index = CPIA2_VC_VC_VFRACT;
+	cmd.buffer.registers[i++].value = (u8) 0x81;	/* = 8/1 = 8 (HIBYTE/LOBYTE) */
+
+	cmd.reg_count = i;
+
+	cpia2_send_command(cam, &cmd);
+
+	return i;
+}
+
+
+/******************************************************************************
+ *
+ *  config_sensor_500(cam)
+ *
+ *****************************************************************************/
+static int config_sensor_500(struct camera_data *cam,
+			     int req_width, int req_height)
+{
+	struct cpia2_command cmd;
+	int i = 0;
+	int image_size = VIDEOSIZE_CIF;
+	int image_type = VIDEOSIZE_VGA;
+	int width = req_width;
+	int height = req_height;
+	unsigned int device = cam->params.pnp_id.device_type;
+
+	image_size = cpia2_match_video_size(width, height);
+
+	if (width > STV_IMAGE_CIF_COLS || height > STV_IMAGE_CIF_ROWS)
+		image_type = VIDEOSIZE_VGA;
+	else if (width > STV_IMAGE_QVGA_COLS || height > STV_IMAGE_QVGA_ROWS)
+		image_type = VIDEOSIZE_CIF;
+	else if (width > STV_IMAGE_QCIF_COLS || height > STV_IMAGE_QCIF_ROWS)
+		image_type = VIDEOSIZE_QVGA;
+	else
+		image_type = VIDEOSIZE_QCIF;
+
+	if (image_size >= 0) {
+		set_vw_size(cam, image_size);
+		width = cam->params.roi.width;
+		height = cam->params.roi.height;
+	} else {
+		ERR("ConfigSensor500 failed\n");
+		return -EINVAL;
+	}
+
+	DBG("image_size = %d, width = %d, height = %d, type = %d\n",
+	    image_size, width, height, image_type);
+
+	cmd.req_mode = CAMERAACCESS_TYPE_RANDOM | CAMERAACCESS_VC;
+	cmd.direction = TRANSFER_WRITE;
+	i = 0;
+
+	/* VC Format */
+	cmd.buffer.registers[i].index = CPIA2_VC_VC_FORMAT;
+	cmd.buffer.registers[i].value = (u8) CPIA2_VC_VC_FORMAT_UFIRST;
+	if (image_type == VIDEOSIZE_QCIF)
+		cmd.buffer.registers[i].value |= (u8) CPIA2_VC_VC_FORMAT_DECIMATING;
+	i++;
+
+	/* VC Clocks */
+	cmd.buffer.registers[i].index = CPIA2_VC_VC_CLOCKS;
+	if (device == DEVICE_STV_672) {
+		if (image_type == VIDEOSIZE_VGA)
+			cmd.buffer.registers[i].value =
+				(u8)CPIA2_VC_VC_CLOCKS_LOGDIV1;
+		else
+			cmd.buffer.registers[i].value =
+				(u8)(CPIA2_VC_VC_672_CLOCKS_SCALING |
+				     CPIA2_VC_VC_CLOCKS_LOGDIV3);
+	} else {
+		if (image_type == VIDEOSIZE_VGA)
+			cmd.buffer.registers[i].value =
+				(u8)CPIA2_VC_VC_CLOCKS_LOGDIV0;
+		else
+			cmd.buffer.registers[i].value =
+				(u8)(CPIA2_VC_VC_676_CLOCKS_SCALING |
+				     CPIA2_VC_VC_CLOCKS_LOGDIV2);
+	}
+	i++;
+
+	DBG("VC_CLOCKS = 0x%X\n", cmd.buffer.registers[i-1].value);
+
+	/* Input width from VP */
+	cmd.buffer.registers[i].index = CPIA2_VC_VC_IHSIZE_LO;
+	if (image_type == VIDEOSIZE_VGA)
+		cmd.buffer.registers[i].value =
+		    (u8) (STV_IMAGE_VGA_COLS / 4);
+	else
+		cmd.buffer.registers[i].value =
+		    (u8) (STV_IMAGE_QVGA_COLS / 4);
+	i++;
+	DBG("Input width = %d\n", cmd.buffer.registers[i-1].value);
+
+	/* Timings */
+	cmd.buffer.registers[i].index = CPIA2_VC_VC_XLIM_HI;
+	if (image_type == VIDEOSIZE_VGA)
+		cmd.buffer.registers[i++].value = (u8) 2;
+	else
+		cmd.buffer.registers[i++].value = (u8) 1;
+
+	cmd.buffer.registers[i].index = CPIA2_VC_VC_XLIM_LO;
+	if (image_type == VIDEOSIZE_VGA)
+		cmd.buffer.registers[i++].value = (u8) 250;
+	else if (image_type == VIDEOSIZE_QVGA)
+		cmd.buffer.registers[i++].value = (u8) 125;
+	else
+		cmd.buffer.registers[i++].value = (u8) 160;
+
+	cmd.buffer.registers[i].index = CPIA2_VC_VC_YLIM_HI;
+	if (image_type == VIDEOSIZE_VGA)
+		cmd.buffer.registers[i++].value = (u8) 2;
+	else
+		cmd.buffer.registers[i++].value = (u8) 1;
+
+	cmd.buffer.registers[i].index = CPIA2_VC_VC_YLIM_LO;
+	if (image_type == VIDEOSIZE_VGA)
+		cmd.buffer.registers[i++].value = (u8) 12;
+	else if (image_type == VIDEOSIZE_QVGA)
+		cmd.buffer.registers[i++].value = (u8) 64;
+	else
+		cmd.buffer.registers[i++].value = (u8) 6;
+
+	/* Output Image Size */
+	cmd.buffer.registers[i].index = CPIA2_VC_VC_OHSIZE;
+	if (image_type == VIDEOSIZE_QCIF)
+		cmd.buffer.registers[i++].value = STV_IMAGE_CIF_COLS  / 4;
+	else
+		cmd.buffer.registers[i++].value = width / 4;
+
+	cmd.buffer.registers[i].index = CPIA2_VC_VC_OVSIZE;
+	if (image_type == VIDEOSIZE_QCIF)
+		cmd.buffer.registers[i++].value = STV_IMAGE_CIF_ROWS  / 4;
+	else
+		cmd.buffer.registers[i++].value = height / 4;
+
+	/* Cropping */
+	cmd.buffer.registers[i].index = CPIA2_VC_VC_HCROP;
+	if (image_type == VIDEOSIZE_VGA)
+		cmd.buffer.registers[i++].value =
+		    (u8) (((STV_IMAGE_VGA_COLS / 4) - (width / 4)) / 2);
+	else if (image_type == VIDEOSIZE_QVGA)
+		cmd.buffer.registers[i++].value =
+		    (u8) (((STV_IMAGE_QVGA_COLS / 4) - (width / 4)) / 2);
+	else if (image_type == VIDEOSIZE_CIF)
+		cmd.buffer.registers[i++].value =
+		    (u8) (((STV_IMAGE_CIF_COLS / 4) - (width / 4)) / 2);
+	else /*if (image_type == VIDEOSIZE_QCIF)*/
+		cmd.buffer.registers[i++].value =
+			(u8) (((STV_IMAGE_QCIF_COLS / 4) - (width / 4)) / 2);
+
+	cmd.buffer.registers[i].index = CPIA2_VC_VC_VCROP;
+	if (image_type == VIDEOSIZE_VGA)
+		cmd.buffer.registers[i++].value =
+		    (u8) (((STV_IMAGE_VGA_ROWS / 4) - (height / 4)) / 2);
+	else if (image_type == VIDEOSIZE_QVGA)
+		cmd.buffer.registers[i++].value =
+		    (u8) (((STV_IMAGE_QVGA_ROWS / 4) - (height / 4)) / 2);
+	else if (image_type == VIDEOSIZE_CIF)
+		cmd.buffer.registers[i++].value =
+		    (u8) (((STV_IMAGE_CIF_ROWS / 4) - (height / 4)) / 2);
+	else /*if (image_type == VIDEOSIZE_QCIF)*/
+		cmd.buffer.registers[i++].value =
+		    (u8) (((STV_IMAGE_QCIF_ROWS / 4) - (height / 4)) / 2);
+
+	/* Scaling registers (defaults) */
+	cmd.buffer.registers[i].index = CPIA2_VC_VC_HPHASE;
+	if (image_type == VIDEOSIZE_CIF || image_type == VIDEOSIZE_QCIF)
+		cmd.buffer.registers[i++].value = (u8) 36;
+	else
+		cmd.buffer.registers[i++].value = (u8) 0;
+
+	cmd.buffer.registers[i].index = CPIA2_VC_VC_VPHASE;
+	if (image_type == VIDEOSIZE_CIF || image_type == VIDEOSIZE_QCIF)
+		cmd.buffer.registers[i++].value = (u8) 32;
+	else
+		cmd.buffer.registers[i++].value = (u8) 0;
+
+	cmd.buffer.registers[i].index = CPIA2_VC_VC_HISPAN;
+	if (image_type == VIDEOSIZE_CIF || image_type == VIDEOSIZE_QCIF)
+		cmd.buffer.registers[i++].value = (u8) 26;
+	else
+		cmd.buffer.registers[i++].value = (u8) 31;
+
+	cmd.buffer.registers[i].index = CPIA2_VC_VC_VISPAN;
+	if (image_type == VIDEOSIZE_CIF || image_type == VIDEOSIZE_QCIF)
+		cmd.buffer.registers[i++].value = (u8) 21;
+	else
+		cmd.buffer.registers[i++].value = (u8) 31;
+
+	cmd.buffer.registers[i].index = CPIA2_VC_VC_HICROP;
+	cmd.buffer.registers[i++].value = (u8) 0;
+
+	cmd.buffer.registers[i].index = CPIA2_VC_VC_VICROP;
+	cmd.buffer.registers[i++].value = (u8) 0;
+
+	cmd.buffer.registers[i].index = CPIA2_VC_VC_HFRACT;
+	if (image_type == VIDEOSIZE_CIF || image_type == VIDEOSIZE_QCIF)
+		cmd.buffer.registers[i++].value = (u8) 0x2B;	/* 2/11 */
+	else
+		cmd.buffer.registers[i++].value = (u8) 0x81;	/* 8/1 */
+
+	cmd.buffer.registers[i].index = CPIA2_VC_VC_VFRACT;
+	if (image_type == VIDEOSIZE_CIF || image_type == VIDEOSIZE_QCIF)
+		cmd.buffer.registers[i++].value = (u8) 0x13;	/* 1/3 */
+	else
+		cmd.buffer.registers[i++].value = (u8) 0x81;	/* 8/1 */
+
+	cmd.reg_count = i;
+
+	cpia2_send_command(cam, &cmd);
+
+	return i;
+}
+
+
+/******************************************************************************
+ *
+ *  setallproperties
+ *
+ *  This sets all user changeable properties to the values in cam->params.
+ *****************************************************************************/
+int set_all_properties(struct camera_data *cam)
+{
+	/**
+	 * Don't set target_kb here, it will be set later.
+	 * framerate and user_mode were already set (set_default_user_mode).
+	 **/
+
+	cpia2_set_color_params(cam);
+
+	cpia2_usb_change_streaming_alternate(cam,
+					  cam->params.camera_state.stream_mode);
+
+	cpia2_do_command(cam, CPIA2_CMD_SET_USER_EFFECTS, TRANSFER_WRITE,
+			 cam->params.vp_params.user_effects);
+
+	cpia2_set_flicker_mode(cam,
+			       cam->params.flicker_control.flicker_mode_req);
+
+	cpia2_do_command(cam,
+			 CPIA2_CMD_SET_VC_MP_GPIO_DIRECTION,
+			 TRANSFER_WRITE, cam->params.vp_params.gpio_direction);
+	cpia2_do_command(cam, CPIA2_CMD_SET_VC_MP_GPIO_DATA, TRANSFER_WRITE,
+			 cam->params.vp_params.gpio_data);
+
+	wake_system(cam);
+
+	set_lowlight_boost(cam);
+
+	return 0;
+}
+
+/******************************************************************************
+ *
+ *  cpia2_save_camera_state
+ *
+ *****************************************************************************/
+void cpia2_save_camera_state(struct camera_data *cam)
+{
+	get_color_params(cam);
+	cpia2_do_command(cam, CPIA2_CMD_GET_USER_EFFECTS, TRANSFER_READ, 0);
+	cpia2_do_command(cam, CPIA2_CMD_GET_VC_MP_GPIO_DIRECTION, TRANSFER_READ,
+			 0);
+	cpia2_do_command(cam, CPIA2_CMD_GET_VC_MP_GPIO_DATA, TRANSFER_READ, 0);
+	/* Don't get framerate or target_kb. Trust the values we already have */
+}
+
+/******************************************************************************
+ *
+ *  get_color_params
+ *
+ *****************************************************************************/
+void get_color_params(struct camera_data *cam)
+{
+	cpia2_do_command(cam, CPIA2_CMD_GET_VP_BRIGHTNESS, TRANSFER_READ, 0);
+	cpia2_do_command(cam, CPIA2_CMD_GET_VP_SATURATION, TRANSFER_READ, 0);
+	cpia2_do_command(cam, CPIA2_CMD_GET_CONTRAST, TRANSFER_READ, 0);
+}
+
+/******************************************************************************
+ *
+ *  cpia2_set_color_params
+ *
+ *****************************************************************************/
+void cpia2_set_color_params(struct camera_data *cam)
+{
+	DBG("Setting color params\n");
+	cpia2_set_brightness(cam, cam->params.color_params.brightness);
+	cpia2_set_contrast(cam, cam->params.color_params.contrast);
+	cpia2_set_saturation(cam, cam->params.color_params.saturation);
+}
+
+/******************************************************************************
+ *
+ *  cpia2_set_flicker_mode
+ *
+ *****************************************************************************/
+int cpia2_set_flicker_mode(struct camera_data *cam, int mode)
+{
+	unsigned char cam_reg;
+	int err = 0;
+
+	if(cam->params.pnp_id.device_type != DEVICE_STV_672)
+		return -EINVAL;
+
+	/* Set the appropriate bits in FLICKER_MODES, preserving the rest */
+	if((err = cpia2_do_command(cam, CPIA2_CMD_GET_FLICKER_MODES,
+				   TRANSFER_READ, 0)))
+		return err;
+	cam_reg = cam->params.flicker_control.cam_register;
+
+	switch(mode) {
+	case NEVER_FLICKER:
+		cam_reg |= CPIA2_VP_FLICKER_MODES_NEVER_FLICKER;
+		cam_reg &= ~CPIA2_VP_FLICKER_MODES_50HZ;
+		break;
+	case FLICKER_60:
+		cam_reg &= ~CPIA2_VP_FLICKER_MODES_NEVER_FLICKER;
+		cam_reg &= ~CPIA2_VP_FLICKER_MODES_50HZ;
+		break;
+	case FLICKER_50:
+		cam_reg &= ~CPIA2_VP_FLICKER_MODES_NEVER_FLICKER;
+		cam_reg |= CPIA2_VP_FLICKER_MODES_50HZ;
+		break;
+	default:
+		return -EINVAL;
+	}
+
+	if((err = cpia2_do_command(cam, CPIA2_CMD_SET_FLICKER_MODES,
+				   TRANSFER_WRITE, cam_reg)))
+		return err;
+
+	/* Set the appropriate bits in EXP_MODES, preserving the rest */
+	if((err = cpia2_do_command(cam, CPIA2_CMD_GET_VP_EXP_MODES,
+				   TRANSFER_READ, 0)))
+		return err;
+	cam_reg = cam->params.vp_params.exposure_modes;
+
+	if (mode == NEVER_FLICKER) {
+		cam_reg |= CPIA2_VP_EXPOSURE_MODES_INHIBIT_FLICKER;
+	} else {
+		cam_reg &= ~CPIA2_VP_EXPOSURE_MODES_INHIBIT_FLICKER;
+	}
+
+	if((err = cpia2_do_command(cam, CPIA2_CMD_SET_VP_EXP_MODES,
+				   TRANSFER_WRITE, cam_reg)))
+		return err;
+
+	if((err = cpia2_do_command(cam, CPIA2_CMD_REHASH_VP4,
+				   TRANSFER_WRITE, 1)))
+		return err;
+
+	switch(mode) {
+	case NEVER_FLICKER:
+		cam->params.flicker_control.flicker_mode_req = mode;
+		break;
+	case FLICKER_60:
+		cam->params.flicker_control.flicker_mode_req = mode;
+		cam->params.flicker_control.mains_frequency = 60;
+		break;
+	case FLICKER_50:
+		cam->params.flicker_control.flicker_mode_req = mode;
+		cam->params.flicker_control.mains_frequency = 50;
+		break;
+	default:
+		err = -EINVAL;
+	}
+
+	return err;
+}
+
+/******************************************************************************
+ *
+ *  cpia2_set_property_flip
+ *
+ *****************************************************************************/
+void cpia2_set_property_flip(struct camera_data *cam, int prop_val)
+{
+	unsigned char cam_reg;
+
+	cpia2_do_command(cam, CPIA2_CMD_GET_USER_EFFECTS, TRANSFER_READ, 0);
+	cam_reg = cam->params.vp_params.user_effects;
+
+	if (prop_val)
+	{
+		cam_reg |= CPIA2_VP_USER_EFFECTS_FLIP;
+	}
+	else
+	{
+		cam_reg &= ~CPIA2_VP_USER_EFFECTS_FLIP;
+	}
+	cpia2_do_command(cam, CPIA2_CMD_SET_USER_EFFECTS, TRANSFER_WRITE,
+			 cam_reg);
+}
+
+/******************************************************************************
+ *
+ *  cpia2_set_property_mirror
+ *
+ *****************************************************************************/
+void cpia2_set_property_mirror(struct camera_data *cam, int prop_val)
+{
+	unsigned char cam_reg;
+
+	cpia2_do_command(cam, CPIA2_CMD_GET_USER_EFFECTS, TRANSFER_READ, 0);
+	cam_reg = cam->params.vp_params.user_effects;
+
+	if (prop_val)
+	{
+		cam_reg |= CPIA2_VP_USER_EFFECTS_MIRROR;
+	}
+	else
+	{
+		cam_reg &= ~CPIA2_VP_USER_EFFECTS_MIRROR;
+	}
+	cpia2_do_command(cam, CPIA2_CMD_SET_USER_EFFECTS, TRANSFER_WRITE,
+			 cam_reg);
+}
+
+/******************************************************************************
+ *
+ *  set_target_kb
+ *
+ *  The new Target KB is set in cam->params.vc_params.target_kb and
+ *  activates on reset.
+ *****************************************************************************/
+
+int cpia2_set_target_kb(struct camera_data *cam, unsigned char value)
+{
+	DBG("Requested target_kb = %d\n", value);
+	if (value != cam->params.vc_params.target_kb) {
+
+		cpia2_usb_stream_pause(cam);
+
+		/* reset camera for new target_kb */
+		cam->params.vc_params.target_kb = value;
+		cpia2_reset_camera(cam);
+
+		cpia2_usb_stream_resume(cam);
+	}
+
+	return 0;
+}
+
+/******************************************************************************
+ *
+ *  cpia2_set_gpio
+ *
+ *****************************************************************************/
+int cpia2_set_gpio(struct camera_data *cam, unsigned char setting)
+{
+	int ret;
+
+	/* Set the microport direction (register 0x90, should be defined
+	 * already) to 1 (user output), and set the microport data (0x91) to
+	 * the value in the ioctl argument.
+	 */
+
+	ret = cpia2_do_command(cam,
+			       CPIA2_CMD_SET_VC_MP_GPIO_DIRECTION,
+			       CPIA2_VC_MP_DIR_OUTPUT,
+			       255);
+	if (ret < 0)
+		return ret;
+	cam->params.vp_params.gpio_direction = 255;
+
+	ret = cpia2_do_command(cam,
+			       CPIA2_CMD_SET_VC_MP_GPIO_DATA,
+			       CPIA2_VC_MP_DIR_OUTPUT,
+			       setting);
+	if (ret < 0)
+		return ret;
+	cam->params.vp_params.gpio_data = setting;
+
+	return 0;
+}
+
+/******************************************************************************
+ *
+ *  cpia2_set_fps
+ *
+ *****************************************************************************/
+int cpia2_set_fps(struct camera_data *cam, int framerate)
+{
+	int retval;
+
+	switch(framerate) {
+		case CPIA2_VP_FRAMERATE_30:
+		case CPIA2_VP_FRAMERATE_25:
+			if(cam->params.pnp_id.device_type == DEVICE_STV_672 &&
+			   cam->params.version.sensor_flags ==
+						    CPIA2_VP_SENSOR_FLAGS_500) {
+				return -EINVAL;
+			}
+			/* Fall through */
+		case CPIA2_VP_FRAMERATE_15:
+		case CPIA2_VP_FRAMERATE_12_5:
+		case CPIA2_VP_FRAMERATE_7_5:
+		case CPIA2_VP_FRAMERATE_6_25:
+			break;
+		default:
+			return -EINVAL;
+	}
+
+	if (cam->params.pnp_id.device_type == DEVICE_STV_672 &&
+	    framerate == CPIA2_VP_FRAMERATE_15)
+		framerate = 0; /* Work around bug in VP4 */
+
+	retval = cpia2_do_command(cam,
+				 CPIA2_CMD_FRAMERATE_REQ,
+				 TRANSFER_WRITE,
+				 framerate);
+
+	if(retval == 0)
+		cam->params.vp_params.frame_rate = framerate;
+
+	return retval;
+}
+
+/******************************************************************************
+ *
+ *  cpia2_set_brightness
+ *
+ *****************************************************************************/
+void cpia2_set_brightness(struct camera_data *cam, unsigned char value)
+{
+	/***
+	 * Don't let the register be set to zero - bug in VP4 - flash of full
+	 * brightness
+	 ***/
+	if (cam->params.pnp_id.device_type == DEVICE_STV_672 && value == 0)
+		value++;
+	DBG("Setting brightness to %d (0x%0x)\n", value, value);
+	cpia2_do_command(cam,CPIA2_CMD_SET_VP_BRIGHTNESS, TRANSFER_WRITE,value);
+}
+
+/******************************************************************************
+ *
+ *  cpia2_set_contrast
+ *
+ *****************************************************************************/
+void cpia2_set_contrast(struct camera_data *cam, unsigned char value)
+{
+	DBG("Setting contrast to %d (0x%0x)\n", value, value);
+	cam->params.color_params.contrast = value;
+	cpia2_do_command(cam, CPIA2_CMD_SET_CONTRAST, TRANSFER_WRITE, value);
+}
+
+/******************************************************************************
+ *
+ *  cpia2_set_saturation
+ *
+ *****************************************************************************/
+void cpia2_set_saturation(struct camera_data *cam, unsigned char value)
+{
+	DBG("Setting saturation to %d (0x%0x)\n", value, value);
+	cam->params.color_params.saturation = value;
+	cpia2_do_command(cam,CPIA2_CMD_SET_VP_SATURATION, TRANSFER_WRITE,value);
+}
+
+/******************************************************************************
+ *
+ *  wake_system
+ *
+ *****************************************************************************/
+void wake_system(struct camera_data *cam)
+{
+	cpia2_do_command(cam, CPIA2_CMD_SET_WAKEUP, TRANSFER_WRITE, 0);
+}
+
+/******************************************************************************
+ *
+ *  set_lowlight_boost
+ *
+ *  Valid for STV500 sensor only
+ *****************************************************************************/
+void set_lowlight_boost(struct camera_data *cam)
+{
+	struct cpia2_command cmd;
+
+	if (cam->params.pnp_id.device_type != DEVICE_STV_672 ||
+	    cam->params.version.sensor_flags != CPIA2_VP_SENSOR_FLAGS_500)
+		return;
+
+	cmd.direction = TRANSFER_WRITE;
+	cmd.req_mode = CAMERAACCESS_TYPE_BLOCK | CAMERAACCESS_VP;
+	cmd.reg_count = 3;
+	cmd.start = CPIA2_VP_RAM_ADDR_H;
+
+	cmd.buffer.block_data[0] = 0;	/* High byte of address to write to */
+	cmd.buffer.block_data[1] = 0x59;	/* Low byte of address to write to */
+	cmd.buffer.block_data[2] = 0;	/* High byte of data to write */
+
+	cpia2_send_command(cam, &cmd);
+
+	if (cam->params.vp_params.lowlight_boost) {
+		cmd.buffer.block_data[0] = 0x02;	/* Low byte data to write */
+	} else {
+		cmd.buffer.block_data[0] = 0x06;
+	}
+	cmd.start = CPIA2_VP_RAM_DATA;
+	cmd.reg_count = 1;
+	cpia2_send_command(cam, &cmd);
+
+	/* Rehash the VP4 values */
+	cpia2_do_command(cam, CPIA2_CMD_REHASH_VP4, TRANSFER_WRITE, 1);
+}
+
+/******************************************************************************
+ *
+ *  cpia2_set_format
+ *
+ *  Assumes that new size is already set in param struct.
+ *****************************************************************************/
+void cpia2_set_format(struct camera_data *cam)
+{
+	cam->flush = true;
+
+	cpia2_usb_stream_pause(cam);
+
+	/* reset camera to new size */
+	cpia2_set_low_power(cam);
+	cpia2_reset_camera(cam);
+	cam->flush = false;
+
+	cpia2_dbg_dump_registers(cam);
+
+	cpia2_usb_stream_resume(cam);
+}
+
+/******************************************************************************
+ *
+ * cpia2_dbg_dump_registers
+ *
+ *****************************************************************************/
+void cpia2_dbg_dump_registers(struct camera_data *cam)
+{
+#ifdef _CPIA2_DEBUG_
+	struct cpia2_command cmd;
+
+	if (!(debugs_on & DEBUG_DUMP_REGS))
+		return;
+
+	cmd.direction = TRANSFER_READ;
+
+	/* Start with bank 0 (SYSTEM) */
+	cmd.req_mode = CAMERAACCESS_TYPE_BLOCK | CAMERAACCESS_SYSTEM;
+	cmd.reg_count = 3;
+	cmd.start = 0;
+	cpia2_send_command(cam, &cmd);
+	printk(KERN_DEBUG "System Device Hi      = 0x%X\n",
+	       cmd.buffer.block_data[0]);
+	printk(KERN_DEBUG "System Device Lo      = 0x%X\n",
+	       cmd.buffer.block_data[1]);
+	printk(KERN_DEBUG "System_system control = 0x%X\n",
+	       cmd.buffer.block_data[2]);
+
+	/* Bank 1 (VC) */
+	cmd.req_mode = CAMERAACCESS_TYPE_BLOCK | CAMERAACCESS_VC;
+	cmd.reg_count = 4;
+	cmd.start = 0x80;
+	cpia2_send_command(cam, &cmd);
+	printk(KERN_DEBUG "ASIC_ID       = 0x%X\n",
+	       cmd.buffer.block_data[0]);
+	printk(KERN_DEBUG "ASIC_REV      = 0x%X\n",
+	       cmd.buffer.block_data[1]);
+	printk(KERN_DEBUG "PW_CONTRL     = 0x%X\n",
+	       cmd.buffer.block_data[2]);
+	printk(KERN_DEBUG "WAKEUP        = 0x%X\n",
+	       cmd.buffer.block_data[3]);
+
+	cmd.start = 0xA0;	/* ST_CTRL */
+	cmd.reg_count = 1;
+	cpia2_send_command(cam, &cmd);
+	printk(KERN_DEBUG "Stream ctrl   = 0x%X\n",
+	       cmd.buffer.block_data[0]);
+
+	cmd.start = 0xA4;	/* Stream status */
+	cpia2_send_command(cam, &cmd);
+	printk(KERN_DEBUG "Stream status = 0x%X\n",
+	       cmd.buffer.block_data[0]);
+
+	cmd.start = 0xA8;	/* USB status */
+	cmd.reg_count = 3;
+	cpia2_send_command(cam, &cmd);
+	printk(KERN_DEBUG "USB_CTRL      = 0x%X\n",
+	       cmd.buffer.block_data[0]);
+	printk(KERN_DEBUG "USB_STRM      = 0x%X\n",
+	       cmd.buffer.block_data[1]);
+	printk(KERN_DEBUG "USB_STATUS    = 0x%X\n",
+	       cmd.buffer.block_data[2]);
+
+	cmd.start = 0xAF;	/* USB settings */
+	cmd.reg_count = 1;
+	cpia2_send_command(cam, &cmd);
+	printk(KERN_DEBUG "USB settings  = 0x%X\n",
+	       cmd.buffer.block_data[0]);
+
+	cmd.start = 0xC0;	/* VC stuff */
+	cmd.reg_count = 26;
+	cpia2_send_command(cam, &cmd);
+	printk(KERN_DEBUG "VC Control    = 0x%0X\n",
+	       cmd.buffer.block_data[0]);
+	printk(KERN_DEBUG "VC Format     = 0x%0X\n",
+	       cmd.buffer.block_data[3]);
+	printk(KERN_DEBUG "VC Clocks     = 0x%0X\n",
+	       cmd.buffer.block_data[4]);
+	printk(KERN_DEBUG "VC IHSize     = 0x%0X\n",
+	       cmd.buffer.block_data[5]);
+	printk(KERN_DEBUG "VC Xlim Hi    = 0x%0X\n",
+	       cmd.buffer.block_data[6]);
+	printk(KERN_DEBUG "VC XLim Lo    = 0x%0X\n",
+	       cmd.buffer.block_data[7]);
+	printk(KERN_DEBUG "VC YLim Hi    = 0x%0X\n",
+	       cmd.buffer.block_data[8]);
+	printk(KERN_DEBUG "VC YLim Lo    = 0x%0X\n",
+	       cmd.buffer.block_data[9]);
+	printk(KERN_DEBUG "VC OHSize     = 0x%0X\n",
+	       cmd.buffer.block_data[10]);
+	printk(KERN_DEBUG "VC OVSize     = 0x%0X\n",
+	       cmd.buffer.block_data[11]);
+	printk(KERN_DEBUG "VC HCrop      = 0x%0X\n",
+	       cmd.buffer.block_data[12]);
+	printk(KERN_DEBUG "VC VCrop      = 0x%0X\n",
+	       cmd.buffer.block_data[13]);
+	printk(KERN_DEBUG "VC HPhase     = 0x%0X\n",
+	       cmd.buffer.block_data[14]);
+	printk(KERN_DEBUG "VC VPhase     = 0x%0X\n",
+	       cmd.buffer.block_data[15]);
+	printk(KERN_DEBUG "VC HIspan     = 0x%0X\n",
+	       cmd.buffer.block_data[16]);
+	printk(KERN_DEBUG "VC VIspan     = 0x%0X\n",
+	       cmd.buffer.block_data[17]);
+	printk(KERN_DEBUG "VC HiCrop     = 0x%0X\n",
+	       cmd.buffer.block_data[18]);
+	printk(KERN_DEBUG "VC ViCrop     = 0x%0X\n",
+	       cmd.buffer.block_data[19]);
+	printk(KERN_DEBUG "VC HiFract    = 0x%0X\n",
+	       cmd.buffer.block_data[20]);
+	printk(KERN_DEBUG "VC ViFract    = 0x%0X\n",
+	       cmd.buffer.block_data[21]);
+	printk(KERN_DEBUG "VC JPeg Opt   = 0x%0X\n",
+	       cmd.buffer.block_data[22]);
+	printk(KERN_DEBUG "VC Creep Per  = 0x%0X\n",
+	       cmd.buffer.block_data[23]);
+	printk(KERN_DEBUG "VC User Sq.   = 0x%0X\n",
+	       cmd.buffer.block_data[24]);
+	printk(KERN_DEBUG "VC Target KB  = 0x%0X\n",
+	       cmd.buffer.block_data[25]);
+
+	/*** VP ***/
+	cmd.req_mode = CAMERAACCESS_TYPE_BLOCK | CAMERAACCESS_VP;
+	cmd.reg_count = 14;
+	cmd.start = 0;
+	cpia2_send_command(cam, &cmd);
+
+	printk(KERN_DEBUG "VP Dev Hi     = 0x%0X\n",
+	       cmd.buffer.block_data[0]);
+	printk(KERN_DEBUG "VP Dev Lo     = 0x%0X\n",
+	       cmd.buffer.block_data[1]);
+	printk(KERN_DEBUG "VP Sys State  = 0x%0X\n",
+	       cmd.buffer.block_data[2]);
+	printk(KERN_DEBUG "VP Sys Ctrl   = 0x%0X\n",
+	       cmd.buffer.block_data[3]);
+	printk(KERN_DEBUG "VP Sensor flg = 0x%0X\n",
+	       cmd.buffer.block_data[5]);
+	printk(KERN_DEBUG "VP Sensor Rev = 0x%0X\n",
+	       cmd.buffer.block_data[6]);
+	printk(KERN_DEBUG "VP Dev Config = 0x%0X\n",
+	       cmd.buffer.block_data[7]);
+	printk(KERN_DEBUG "VP GPIO_DIR   = 0x%0X\n",
+	       cmd.buffer.block_data[8]);
+	printk(KERN_DEBUG "VP GPIO_DATA  = 0x%0X\n",
+	       cmd.buffer.block_data[9]);
+	printk(KERN_DEBUG "VP Ram ADDR H = 0x%0X\n",
+	       cmd.buffer.block_data[10]);
+	printk(KERN_DEBUG "VP Ram ADDR L = 0x%0X\n",
+	       cmd.buffer.block_data[11]);
+	printk(KERN_DEBUG "VP RAM Data   = 0x%0X\n",
+	       cmd.buffer.block_data[12]);
+	printk(KERN_DEBUG "Do Call       = 0x%0X\n",
+	       cmd.buffer.block_data[13]);
+
+	if (cam->params.pnp_id.device_type == DEVICE_STV_672) {
+		cmd.reg_count = 9;
+		cmd.start = 0x0E;
+		cpia2_send_command(cam, &cmd);
+		printk(KERN_DEBUG "VP Clock Ctrl = 0x%0X\n",
+		       cmd.buffer.block_data[0]);
+		printk(KERN_DEBUG "VP Patch Rev  = 0x%0X\n",
+		       cmd.buffer.block_data[1]);
+		printk(KERN_DEBUG "VP Vid Mode   = 0x%0X\n",
+		       cmd.buffer.block_data[2]);
+		printk(KERN_DEBUG "VP Framerate  = 0x%0X\n",
+		       cmd.buffer.block_data[3]);
+		printk(KERN_DEBUG "VP UserEffect = 0x%0X\n",
+		       cmd.buffer.block_data[4]);
+		printk(KERN_DEBUG "VP White Bal  = 0x%0X\n",
+		       cmd.buffer.block_data[5]);
+		printk(KERN_DEBUG "VP WB thresh  = 0x%0X\n",
+		       cmd.buffer.block_data[6]);
+		printk(KERN_DEBUG "VP Exp Modes  = 0x%0X\n",
+		       cmd.buffer.block_data[7]);
+		printk(KERN_DEBUG "VP Exp Target = 0x%0X\n",
+		       cmd.buffer.block_data[8]);
+
+		cmd.reg_count = 1;
+		cmd.start = 0x1B;
+		cpia2_send_command(cam, &cmd);
+		printk(KERN_DEBUG "VP FlickerMds = 0x%0X\n",
+		       cmd.buffer.block_data[0]);
+	} else {
+		cmd.reg_count = 8 ;
+		cmd.start = 0x0E;
+		cpia2_send_command(cam, &cmd);
+		printk(KERN_DEBUG "VP Clock Ctrl = 0x%0X\n",
+		       cmd.buffer.block_data[0]);
+		printk(KERN_DEBUG "VP Patch Rev  = 0x%0X\n",
+		       cmd.buffer.block_data[1]);
+		printk(KERN_DEBUG "VP Vid Mode   = 0x%0X\n",
+		       cmd.buffer.block_data[5]);
+		printk(KERN_DEBUG "VP Framerate  = 0x%0X\n",
+		       cmd.buffer.block_data[6]);
+		printk(KERN_DEBUG "VP UserEffect = 0x%0X\n",
+		       cmd.buffer.block_data[7]);
+
+		cmd.reg_count = 1;
+		cmd.start = CPIA2_VP5_EXPOSURE_TARGET;
+		cpia2_send_command(cam, &cmd);
+		printk(KERN_DEBUG "VP5 Exp Target= 0x%0X\n",
+		       cmd.buffer.block_data[0]);
+
+		cmd.reg_count = 4;
+		cmd.start = 0x3A;
+		cpia2_send_command(cam, &cmd);
+		printk(KERN_DEBUG "VP5 MY Black  = 0x%0X\n",
+		       cmd.buffer.block_data[0]);
+		printk(KERN_DEBUG "VP5 MCY Range = 0x%0X\n",
+		       cmd.buffer.block_data[1]);
+		printk(KERN_DEBUG "VP5 MYCEILING = 0x%0X\n",
+		       cmd.buffer.block_data[2]);
+		printk(KERN_DEBUG "VP5 MCUV Sat  = 0x%0X\n",
+		       cmd.buffer.block_data[3]);
+	}
+#endif
+}
+
+/******************************************************************************
+ *
+ *  reset_camera_struct
+ *
+ *  Sets all values to the defaults
+ *****************************************************************************/
+void reset_camera_struct(struct camera_data *cam)
+{
+	/***
+	 * The following parameter values are the defaults from the register map.
+	 ***/
+	cam->params.color_params.brightness = DEFAULT_BRIGHTNESS;
+	cam->params.color_params.contrast = DEFAULT_CONTRAST;
+	cam->params.color_params.saturation = DEFAULT_SATURATION;
+	cam->params.vp_params.lowlight_boost = 0;
+
+	/* FlickerModes */
+	cam->params.flicker_control.flicker_mode_req = NEVER_FLICKER;
+	cam->params.flicker_control.mains_frequency = 60;
+
+	/* jpeg params */
+	cam->params.compression.jpeg_options = CPIA2_VC_VC_JPEG_OPT_DEFAULT;
+	cam->params.compression.creep_period = 2;
+	cam->params.compression.user_squeeze = 20;
+	cam->params.compression.inhibit_htables = false;
+
+	/* gpio params */
+	cam->params.vp_params.gpio_direction = 0;	/* write, the default safe mode */
+	cam->params.vp_params.gpio_data = 0;
+
+	/* Target kb params */
+	cam->params.vc_params.target_kb = DEFAULT_TARGET_KB;
+
+	/***
+	 * Set Sensor FPS as fast as possible.
+	 ***/
+	if(cam->params.pnp_id.device_type == DEVICE_STV_672) {
+		if(cam->params.version.sensor_flags == CPIA2_VP_SENSOR_FLAGS_500)
+			cam->params.vp_params.frame_rate = CPIA2_VP_FRAMERATE_15;
+		else
+			cam->params.vp_params.frame_rate = CPIA2_VP_FRAMERATE_30;
+	} else {
+		cam->params.vp_params.frame_rate = CPIA2_VP_FRAMERATE_30;
+	}
+
+	/***
+	 * Set default video mode as large as possible :
+	 * for vga sensor set to vga, for cif sensor set to CIF.
+	 ***/
+	if (cam->params.version.sensor_flags == CPIA2_VP_SENSOR_FLAGS_500) {
+		cam->sensor_type = CPIA2_SENSOR_500;
+		cam->video_size = VIDEOSIZE_VGA;
+		cam->params.roi.width = STV_IMAGE_VGA_COLS;
+		cam->params.roi.height = STV_IMAGE_VGA_ROWS;
+	} else {
+		cam->sensor_type = CPIA2_SENSOR_410;
+		cam->video_size = VIDEOSIZE_CIF;
+		cam->params.roi.width = STV_IMAGE_CIF_COLS;
+		cam->params.roi.height = STV_IMAGE_CIF_ROWS;
+	}
+
+	/***
+	 * Fill in the v4l structures.  video_cap is filled in inside the VIDIOCCAP
+	 * Ioctl.  Here, just do the window and picture stucts.
+	 ***/
+	cam->vp.palette = (u16) VIDEO_PALETTE_RGB24;	/* Is this right? */
+	cam->vp.brightness = (u16) cam->params.color_params.brightness * 256;
+	cam->vp.colour = (u16) cam->params.color_params.saturation * 256;
+	cam->vp.contrast = (u16) cam->params.color_params.contrast * 256;
+
+	cam->vw.x = 0;
+	cam->vw.y = 0;
+	cam->vw.width = cam->params.roi.width;
+	cam->vw.height = cam->params.roi.height;
+	cam->vw.flags = 0;
+	cam->vw.clipcount = 0;
+
+	return;
+}
+
+/******************************************************************************
+ *
+ *  cpia2_init_camera_struct
+ *
+ *  Initializes camera struct, does not call reset to fill in defaults.
+ *****************************************************************************/
+struct camera_data *cpia2_init_camera_struct(void)
+{
+	struct camera_data *cam;
+
+	cam = kmalloc(sizeof(*cam), GFP_KERNEL);
+
+	if (!cam) {
+		ERR("couldn't kmalloc cpia2 struct\n");
+		return NULL;
+	}
+
+	/* Default everything to 0 */
+	memset(cam, 0, sizeof(struct camera_data));
+
+	cam->present = 1;
+	init_MUTEX(&cam->busy_lock);
+	init_waitqueue_head(&cam->wq_stream);
+
+	return cam;
+}
+
+/******************************************************************************
+ *
+ *  cpia2_init_camera
+ *
+ *  Initializes camera.
+ *****************************************************************************/
+int cpia2_init_camera(struct camera_data *cam)
+{
+	DBG("Start\n");
+
+	cam->mmapped = false;
+
+	/* Get sensor and asic types before reset. */
+	cpia2_set_high_power(cam);
+	cpia2_get_version_info(cam);
+	if (cam->params.version.asic_id != CPIA2_ASIC_672) {
+		ERR("Device IO error (asicID has incorrect value of 0x%X\n",
+		    cam->params.version.asic_id);
+		return -ENODEV;
+	}
+
+	/* Set GPIO direction and data to a safe state. */
+	cpia2_do_command(cam, CPIA2_CMD_SET_VC_MP_GPIO_DIRECTION,
+			 TRANSFER_WRITE, 0);
+	cpia2_do_command(cam, CPIA2_CMD_SET_VC_MP_GPIO_DATA,
+			 TRANSFER_WRITE, 0);
+
+	/* resetting struct requires version info for sensor and asic types */
+	reset_camera_struct(cam);
+
+	cpia2_set_low_power(cam);
+
+	DBG("End\n");
+
+	return 0;
+}
+
+/******************************************************************************
+ *
+ *  cpia2_allocate_buffers
+ *
+ *****************************************************************************/
+int cpia2_allocate_buffers(struct camera_data *cam)
+{
+	int i;
+
+	if(!cam->buffers) {
+		u32 size = cam->num_frames*sizeof(struct framebuf);
+		cam->buffers = kmalloc(size, GFP_KERNEL);
+		if(!cam->buffers) {
+			ERR("couldn't kmalloc frame buffer structures\n");
+			return -ENOMEM;
+		}
+	}
+
+	if(!cam->frame_buffer) {
+		cam->frame_buffer = rvmalloc(cam->frame_size*cam->num_frames);
+		if (!cam->frame_buffer) {
+			ERR("couldn't vmalloc frame buffer data area\n");
+			kfree(cam->buffers);
+			cam->buffers = NULL;
+			return -ENOMEM;
+		}
+	}
+
+	for(i=0; i<cam->num_frames-1; ++i) {
+		cam->buffers[i].next = &cam->buffers[i+1];
+		cam->buffers[i].data = cam->frame_buffer +i*cam->frame_size;
+		cam->buffers[i].status = FRAME_EMPTY;
+		cam->buffers[i].length = 0;
+		cam->buffers[i].max_length = 0;
+		cam->buffers[i].num = i;
+	}
+	cam->buffers[i].next = cam->buffers;
+	cam->buffers[i].data = cam->frame_buffer +i*cam->frame_size;
+	cam->buffers[i].status = FRAME_EMPTY;
+	cam->buffers[i].length = 0;
+	cam->buffers[i].max_length = 0;
+	cam->buffers[i].num = i;
+	cam->curbuff = cam->buffers;
+	cam->workbuff = cam->curbuff->next;
+	DBG("buffers=%p, curbuff=%p, workbuff=%p\n", cam->buffers, cam->curbuff,
+	    cam->workbuff);
+	return 0;
+}
+
+/******************************************************************************
+ *
+ *  cpia2_free_buffers
+ *
+ *****************************************************************************/
+void cpia2_free_buffers(struct camera_data *cam)
+{
+	if(cam->buffers) {
+		kfree(cam->buffers);
+		cam->buffers = NULL;
+	}
+	if(cam->frame_buffer) {
+		rvfree(cam->frame_buffer, cam->frame_size*cam->num_frames);
+		cam->frame_buffer = NULL;
+	}
+}
+
+/******************************************************************************
+ *
+ *  cpia2_read
+ *
+ *****************************************************************************/
+long cpia2_read(struct camera_data *cam,
+		char __user *buf, unsigned long count, int noblock)
+{
+	struct framebuf *frame;
+	if (!count) {
+		return 0;
+	}
+
+	if (!buf) {
+		ERR("%s: buffer NULL\n",__FUNCTION__);
+		return -EINVAL;
+	}
+
+	if (!cam) {
+		ERR("%s: Internal error, camera_data NULL!\n",__FUNCTION__);
+		return -EINVAL;
+	}
+
+	/* make this _really_ smp and multithread-safe */
+	if (down_interruptible(&cam->busy_lock))
+		return -ERESTARTSYS;
+
+	if (!cam->present) {
+		LOG("%s: camera removed\n",__FUNCTION__);
+		up(&cam->busy_lock);
+		return 0;	/* EOF */
+	}
+
+	if(!cam->streaming) {
+		/* Start streaming */
+		cpia2_usb_stream_start(cam,
+				       cam->params.camera_state.stream_mode);
+	}
+
+	/* Copy cam->curbuff in case it changes while we're processing */
+	frame = cam->curbuff;
+	if (noblock && frame->status != FRAME_READY) {
+		up(&cam->busy_lock);
+		return -EAGAIN;
+	}
+
+	if(frame->status != FRAME_READY) {
+		up(&cam->busy_lock);
+		wait_event_interruptible(cam->wq_stream,
+			       !cam->present ||
+			       (frame = cam->curbuff)->status == FRAME_READY);
+		if (signal_pending(current))
+			return -ERESTARTSYS;
+		/* make this _really_ smp and multithread-safe */
+		if (down_interruptible(&cam->busy_lock)) {
+			return -ERESTARTSYS;
+		}
+		if(!cam->present) {
+			up(&cam->busy_lock);
+			return 0;
+		}
+	}
+
+	/* copy data to user space */
+	if (frame->length > count) {
+		up(&cam->busy_lock);
+		return -EFAULT;
+	}
+	if (copy_to_user(buf, frame->data, frame->length)) {
+		up(&cam->busy_lock);
+		return -EFAULT;
+	}
+
+	count = frame->length;
+
+	frame->status = FRAME_EMPTY;
+
+	up(&cam->busy_lock);
+	return count;
+}
+
+/******************************************************************************
+ *
+ *  cpia2_poll
+ *
+ *****************************************************************************/
+unsigned int cpia2_poll(struct camera_data *cam, struct file *filp,
+			poll_table *wait)
+{
+	unsigned int status=0;
+
+	if(!cam) {
+		ERR("%s: Internal error, camera_data not found!\n",__FUNCTION__);
+		return POLLERR;
+	}
+
+	down(&cam->busy_lock);
+
+	if(!cam->present) {
+		up(&cam->busy_lock);
+		return POLLHUP;
+	}
+
+	if(!cam->streaming) {
+		/* Start streaming */
+		cpia2_usb_stream_start(cam,
+				       cam->params.camera_state.stream_mode);
+	}
+
+	up(&cam->busy_lock);
+	poll_wait(filp, &cam->wq_stream, wait);
+	down(&cam->busy_lock);
+
+	if(!cam->present)
+		status = POLLHUP;
+	else if(cam->curbuff->status == FRAME_READY)
+		status = POLLIN | POLLRDNORM;
+
+	up(&cam->busy_lock);
+	return status;
+}
+
+/******************************************************************************
+ *
+ *  cpia2_remap_buffer
+ *
+ *****************************************************************************/
+int cpia2_remap_buffer(struct camera_data *cam, struct vm_area_struct *vma)
+{
+	const char *adr = (const char *)vma->vm_start;
+	unsigned long size = vma->vm_end-vma->vm_start;
+	unsigned long start_offset = vma->vm_pgoff << PAGE_SHIFT;
+	unsigned long start = (unsigned long) adr;
+	unsigned long page, pos;
+
+	if (!cam)
+		return -ENODEV;
+
+	DBG("mmap offset:%ld size:%ld\n", start_offset, size);
+
+	/* make this _really_ smp-safe */
+	if (down_interruptible(&cam->busy_lock))
+		return -ERESTARTSYS;
+
+	if (!cam->present) {
+		up(&cam->busy_lock);
+		return -ENODEV;
+	}
+
+	if (size > cam->frame_size*cam->num_frames  ||
+	    (start_offset % cam->frame_size) != 0 ||
+	    (start_offset+size > cam->frame_size*cam->num_frames)) {
+		up(&cam->busy_lock);
+		return -EINVAL;
+	}
+
+	pos = ((unsigned long) (cam->frame_buffer)) + start_offset;
+	while (size > 0) {
+		page = kvirt_to_pa(pos);
+		if (remap_pfn_range(vma, start, page >> PAGE_SHIFT, PAGE_SIZE, PAGE_SHARED)) {
+			up(&cam->busy_lock);
+			return -EAGAIN;
+		}
+		start += PAGE_SIZE;
+		pos += PAGE_SIZE;
+		if (size > PAGE_SIZE)
+			size -= PAGE_SIZE;
+		else
+			size = 0;
+	}
+
+	cam->mmapped = true;
+	up(&cam->busy_lock);
+	return 0;
+}
+
diff --git a/drivers/media/video/cpia2/cpia2_registers.h b/drivers/media/video/cpia2/cpia2_registers.h
new file mode 100644
index 00000000000000..3bbec514a967a8
--- /dev/null
+++ b/drivers/media/video/cpia2/cpia2_registers.h
@@ -0,0 +1,476 @@
+/****************************************************************************
+ *
+ *  Filename: cpia2registers.h
+ *
+ *  Copyright 2001, STMicrolectronics, Inc.
+ *
+ *  Description:
+ *     Definitions for the CPia2 register set
+ *
+ *  This program is free software; you can redistribute it and/or modify
+ *  it under the terms of the GNU General Public License as published by
+ *  the Free Software Foundation; either version 2 of the License, or
+ *  (at your option) any later version.
+ *
+ *  This program is distributed in the hope that it will be useful,
+ *  but WITHOUT ANY WARRANTY; without even the implied warranty of
+ *  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ *  GNU General Public License for more details.
+ *
+ *  You should have received a copy of the GNU General Public License
+ *  along with this program; if not, write to the Free Software
+ *  Foundation, Inc., 675 Mass Ave, Cambridge, MA 02139, USA.
+ *
+ ****************************************************************************/
+
+#ifndef CPIA2_REGISTER_HEADER
+#define CPIA2_REGISTER_HEADER
+
+/***
+ * System register set (Bank 0)
+ ***/
+#define CPIA2_SYSTEM_DEVICE_HI                     0x00
+#define CPIA2_SYSTEM_DEVICE_LO                     0x01
+
+#define CPIA2_SYSTEM_SYSTEM_CONTROL                0x02
+#define CPIA2_SYSTEM_CONTROL_LOW_POWER       0x00
+#define CPIA2_SYSTEM_CONTROL_HIGH_POWER      0x01
+#define CPIA2_SYSTEM_CONTROL_SUSPEND         0x02
+#define CPIA2_SYSTEM_CONTROL_V2W_ERR         0x10
+#define CPIA2_SYSTEM_CONTROL_RB_ERR          0x10
+#define CPIA2_SYSTEM_CONTROL_CLEAR_ERR       0x80
+
+#define CPIA2_SYSTEM_INT_PACKET_CTRL                0x04
+#define CPIA2_SYSTEM_INT_PACKET_CTRL_ENABLE_SW_XX 0x01
+#define CPIA2_SYSTEM_INT_PACKET_CTRL_ENABLE_EOF   0x02
+#define CPIA2_SYSTEM_INT_PACKET_CTRL_ENABLE_INT1  0x04
+
+#define CPIA2_SYSTEM_CACHE_CTRL                     0x05
+#define CPIA2_SYSTEM_CACHE_CTRL_CACHE_RESET      0x01
+#define CPIA2_SYSTEM_CACHE_CTRL_CACHE_FLUSH      0x02
+
+#define CPIA2_SYSTEM_SERIAL_CTRL                    0x06
+#define CPIA2_SYSTEM_SERIAL_CTRL_NULL_CMD        0x00
+#define CPIA2_SYSTEM_SERIAL_CTRL_START_CMD       0x01
+#define CPIA2_SYSTEM_SERIAL_CTRL_STOP_CMD        0x02
+#define CPIA2_SYSTEM_SERIAL_CTRL_WRITE_CMD       0x03
+#define CPIA2_SYSTEM_SERIAL_CTRL_READ_ACK_CMD    0x04
+#define CPIA2_SYSTEM_SERIAL_CTRL_READ_NACK_CMD   0x05
+
+#define CPIA2_SYSTEM_SERIAL_DATA                     0x07
+
+#define CPIA2_SYSTEM_VP_SERIAL_ADDR                  0x08
+
+/***
+ * I2C addresses for various devices in CPiA2
+ ***/
+#define CPIA2_SYSTEM_VP_SERIAL_ADDR_SENSOR           0x20
+#define CPIA2_SYSTEM_VP_SERIAL_ADDR_VP               0x88
+#define CPIA2_SYSTEM_VP_SERIAL_ADDR_676_VP           0x8A
+
+#define CPIA2_SYSTEM_SPARE_REG1                      0x09
+#define CPIA2_SYSTEM_SPARE_REG2                      0x0A
+#define CPIA2_SYSTEM_SPARE_REG3                      0x0B
+
+#define CPIA2_SYSTEM_MC_PORT_0                       0x0C
+#define CPIA2_SYSTEM_MC_PORT_1                       0x0D
+#define CPIA2_SYSTEM_MC_PORT_2                       0x0E
+#define CPIA2_SYSTEM_MC_PORT_3                       0x0F
+
+#define CPIA2_SYSTEM_STATUS_PKT                      0x20
+#define CPIA2_SYSTEM_STATUS_PKT_END                  0x27
+
+#define CPIA2_SYSTEM_DESCRIP_VID_HI                  0x30
+#define CPIA2_SYSTEM_DESCRIP_VID_LO                  0x31
+#define CPIA2_SYSTEM_DESCRIP_PID_HI                  0x32
+#define CPIA2_SYSTEM_DESCRIP_PID_LO                  0x33
+
+#define CPIA2_SYSTEM_FW_VERSION_HI                   0x34
+#define CPIA2_SYSTEM_FW_VERSION_LO                   0x35
+
+#define CPIA2_SYSTEM_CACHE_START_INDEX               0x80
+#define CPIA2_SYSTEM_CACHE_MAX_WRITES                0x10
+
+/***
+ * VC register set (Bank 1)
+ ***/
+#define CPIA2_VC_ASIC_ID                 0x80
+
+#define CPIA2_VC_ASIC_REV                0x81
+
+#define CPIA2_VC_PW_CTRL                 0x82
+#define CPIA2_VC_PW_CTRL_COLDSTART      0x01
+#define CPIA2_VC_PW_CTRL_CP_CLK_EN      0x02
+#define CPIA2_VC_PW_CTRL_VP_RESET_N     0x04
+#define CPIA2_VC_PW_CTRL_VC_CLK_EN      0x08
+#define CPIA2_VC_PW_CTRL_VC_RESET_N     0x10
+#define CPIA2_VC_PW_CTRL_GOTO_SUSPEND   0x20
+#define CPIA2_VC_PW_CTRL_UDC_SUSPEND    0x40
+#define CPIA2_VC_PW_CTRL_PWR_DOWN       0x80
+
+#define CPIA2_VC_WAKEUP                   0x83
+#define CPIA2_VC_WAKEUP_SW_ENABLE       0x01
+#define CPIA2_VC_WAKEUP_XX_ENABLE       0x02
+#define CPIA2_VC_WAKEUP_SW_ATWAKEUP     0x04
+#define CPIA2_VC_WAKEUP_XX_ATWAKEUP     0x08
+
+#define CPIA2_VC_CLOCK_CTRL               0x84
+#define CPIA2_VC_CLOCK_CTRL_TESTUP72    0x01
+
+#define CPIA2_VC_INT_ENABLE                0x88
+#define CPIA2_VC_INT_ENABLE_XX_IE       0x01
+#define CPIA2_VC_INT_ENABLE_SW_IE       0x02
+#define CPIA2_VC_INT_ENABLE_VC_IE       0x04
+#define CPIA2_VC_INT_ENABLE_USBDATA_IE  0x08
+#define CPIA2_VC_INT_ENABLE_USBSETUP_IE 0x10
+#define CPIA2_VC_INT_ENABLE_USBCFG_IE   0x20
+
+#define CPIA2_VC_INT_FLAG                  0x89
+#define CPIA2_VC_INT_ENABLE_XX_FLAG       0x01
+#define CPIA2_VC_INT_ENABLE_SW_FLAG       0x02
+#define CPIA2_VC_INT_ENABLE_VC_FLAG       0x04
+#define CPIA2_VC_INT_ENABLE_USBDATA_FLAG  0x08
+#define CPIA2_VC_INT_ENABLE_USBSETUP_FLAG 0x10
+#define CPIA2_VC_INT_ENABLE_USBCFG_FLAG   0x20
+#define CPIA2_VC_INT_ENABLE_SET_RESET_BIT 0x80
+
+#define CPIA2_VC_INT_STATE                 0x8A
+#define CPIA2_VC_INT_STATE_XX_STATE     0x01
+#define CPIA2_VC_INT_STATE_SW_STATE     0x02
+
+#define CPIA2_VC_MP_DIR                    0x90
+#define CPIA2_VC_MP_DIR_INPUT           0x00
+#define CPIA2_VC_MP_DIR_OUTPUT          0x01
+
+#define CPIA2_VC_MP_DATA                   0x91
+
+#define CPIA2_VC_DP_CTRL                   0x98
+#define CPIA2_VC_DP_CTRL_MODE_0         0x00
+#define CPIA2_VC_DP_CTRL_MODE_A         0x01
+#define CPIA2_VC_DP_CTRL_MODE_B         0x02
+#define CPIA2_VC_DP_CTRL_MODE_C         0x03
+#define CPIA2_VC_DP_CTRL_FAKE_FST       0x04
+
+#define CPIA2_VC_AD_CTRL                   0x99
+#define CPIA2_VC_AD_CTRL_SRC_0          0x00
+#define CPIA2_VC_AD_CTRL_SRC_DIGI_A     0x01
+#define CPIA2_VC_AD_CTRL_SRC_REG        0x02
+#define CPIA2_VC_AD_CTRL_DST_USB        0x00
+#define CPIA2_VC_AD_CTRL_DST_REG        0x04
+
+#define CPIA2_VC_AD_TEST_IN                0x9B
+
+#define CPIA2_VC_AD_TEST_OUT               0x9C
+
+#define CPIA2_VC_AD_STATUS                 0x9D
+#define CPIA2_VC_AD_STATUS_EMPTY        0x01
+#define CPIA2_VC_AD_STATUS_FULL         0x02
+
+#define CPIA2_VC_DP_DATA                   0x9E
+
+#define CPIA2_VC_ST_CTRL                   0xA0
+#define CPIA2_VC_ST_CTRL_SRC_VC         0x00
+#define CPIA2_VC_ST_CTRL_SRC_DP         0x01
+#define CPIA2_VC_ST_CTRL_SRC_REG        0x02
+
+#define CPIA2_VC_ST_CTRL_RAW_SELECT     0x04
+
+#define CPIA2_VC_ST_CTRL_DST_USB        0x00
+#define CPIA2_VC_ST_CTRL_DST_DP         0x08
+#define CPIA2_VC_ST_CTRL_DST_REG        0x10
+
+#define CPIA2_VC_ST_CTRL_FIFO_ENABLE    0x20
+#define CPIA2_VC_ST_CTRL_EOF_DETECT     0x40
+
+#define CPIA2_VC_ST_TEST                   0xA1
+#define CPIA2_VC_ST_TEST_MODE_MANUAL    0x00
+#define CPIA2_VC_ST_TEST_MODE_INCREMENT 0x02
+
+#define CPIA2_VC_ST_TEST_AUTO_FILL      0x08
+
+#define CPIA2_VC_ST_TEST_REPEAT_FIFO    0x10
+
+#define CPIA2_VC_ST_TEST_IN                0xA2
+
+#define CPIA2_VC_ST_TEST_OUT               0xA3
+
+#define CPIA2_VC_ST_STATUS                 0xA4
+#define CPIA2_VC_ST_STATUS_EMPTY        0x01
+#define CPIA2_VC_ST_STATUS_FULL         0x02
+
+#define CPIA2_VC_ST_FRAME_DETECT_1         0xA5
+
+#define CPIA2_VC_ST_FRAME_DETECT_2         0xA6
+
+#define CPIA2_VC_USB_CTRL                    0xA8
+#define CPIA2_VC_USB_CTRL_CMD_STALLED      0x01
+#define CPIA2_VC_USB_CTRL_CMD_READY        0x02
+#define CPIA2_VC_USB_CTRL_CMD_STATUS       0x04
+#define CPIA2_VC_USB_CTRL_CMD_STATUS_DIR   0x08
+#define CPIA2_VC_USB_CTRL_CMD_NO_CLASH     0x10
+#define CPIA2_VC_USB_CTRL_CMD_MICRO_ACCESS 0x80
+
+#define CPIA2_VC_USB_STRM                  0xA9
+#define CPIA2_VC_USB_STRM_ISO_ENABLE    0x01
+#define CPIA2_VC_USB_STRM_BLK_ENABLE    0x02
+#define CPIA2_VC_USB_STRM_INT_ENABLE    0x04
+#define CPIA2_VC_USB_STRM_AUD_ENABLE    0x08
+
+#define CPIA2_VC_USB_STATUS                   0xAA
+#define CPIA2_VC_USB_STATUS_CMD_IN_PROGRESS  0x01
+#define CPIA2_VC_USB_STATUS_CMD_STATUS_STALL 0x02
+#define CPIA2_VC_USB_STATUS_CMD_HANDSHAKE    0x04
+#define CPIA2_VC_USB_STATUS_CMD_OVERRIDE     0x08
+#define CPIA2_VC_USB_STATUS_CMD_FIFO_BUSY    0x10
+#define CPIA2_VC_USB_STATUS_BULK_REPEAT_TXN  0x20
+#define CPIA2_VC_USB_STATUS_CONFIG_DONE      0x40
+#define CPIA2_VC_USB_STATUS_USB_SUSPEND      0x80
+
+#define CPIA2_VC_USB_CMDW                   0xAB
+
+#define CPIA2_VC_USB_DATARW                 0xAC
+
+#define CPIA2_VC_USB_INFO                   0xAD
+
+#define CPIA2_VC_USB_CONFIG                 0xAE
+
+#define CPIA2_VC_USB_SETTINGS                  0xAF
+#define CPIA2_VC_USB_SETTINGS_CONFIG_MASK    0x03
+#define CPIA2_VC_USB_SETTINGS_INTERFACE_MASK 0x0C
+#define CPIA2_VC_USB_SETTINGS_ALTERNATE_MASK 0x70
+
+#define CPIA2_VC_USB_ISOLIM                  0xB0
+
+#define CPIA2_VC_USB_ISOFAILS                0xB1
+
+#define CPIA2_VC_USB_ISOMAXPKTHI             0xB2
+
+#define CPIA2_VC_USB_ISOMAXPKTLO             0xB3
+
+#define CPIA2_VC_V2W_CTRL                    0xB8
+#define CPIA2_VC_V2W_SELECT               0x01
+
+#define CPIA2_VC_V2W_SCL                     0xB9
+
+#define CPIA2_VC_V2W_SDA                     0xBA
+
+#define CPIA2_VC_VC_CTRL                     0xC0
+#define CPIA2_VC_VC_CTRL_RUN              0x01
+#define CPIA2_VC_VC_CTRL_SINGLESHOT       0x02
+#define CPIA2_VC_VC_CTRL_IDLING           0x04
+#define CPIA2_VC_VC_CTRL_INHIBIT_H_TABLES 0x10
+#define CPIA2_VC_VC_CTRL_INHIBIT_Q_TABLES 0x20
+#define CPIA2_VC_VC_CTRL_INHIBIT_PRIVATE  0x40
+
+#define CPIA2_VC_VC_RESTART_IVAL_HI          0xC1
+
+#define CPIA2_VC_VC_RESTART_IVAL_LO          0xC2
+
+#define CPIA2_VC_VC_FORMAT                   0xC3
+#define CPIA2_VC_VC_FORMAT_UFIRST         0x01
+#define CPIA2_VC_VC_FORMAT_MONO           0x02
+#define CPIA2_VC_VC_FORMAT_DECIMATING     0x04
+#define CPIA2_VC_VC_FORMAT_SHORTLINE      0x08
+#define CPIA2_VC_VC_FORMAT_SELFTEST       0x10
+
+#define CPIA2_VC_VC_CLOCKS                         0xC4
+#define CPIA2_VC_VC_CLOCKS_CLKDIV_MASK        0x03
+#define CPIA2_VC_VC_672_CLOCKS_CIF_DIV_BY_3   0x04
+#define CPIA2_VC_VC_672_CLOCKS_SCALING        0x08
+#define CPIA2_VC_VC_CLOCKS_LOGDIV0        0x00
+#define CPIA2_VC_VC_CLOCKS_LOGDIV1        0x01
+#define CPIA2_VC_VC_CLOCKS_LOGDIV2        0x02
+#define CPIA2_VC_VC_CLOCKS_LOGDIV3        0x03
+#define CPIA2_VC_VC_676_CLOCKS_CIF_DIV_BY_3   0x08
+#define CPIA2_VC_VC_676_CLOCKS_SCALING	      0x10
+
+#define CPIA2_VC_VC_IHSIZE_LO                0xC5
+
+#define CPIA2_VC_VC_XLIM_HI                  0xC6
+
+#define CPIA2_VC_VC_XLIM_LO                  0xC7
+
+#define CPIA2_VC_VC_YLIM_HI                  0xC8
+
+#define CPIA2_VC_VC_YLIM_LO                  0xC9
+
+#define CPIA2_VC_VC_OHSIZE                   0xCA
+
+#define CPIA2_VC_VC_OVSIZE                   0xCB
+
+#define CPIA2_VC_VC_HCROP                    0xCC
+
+#define CPIA2_VC_VC_VCROP                    0xCD
+
+#define CPIA2_VC_VC_HPHASE                   0xCE
+
+#define CPIA2_VC_VC_VPHASE                   0xCF
+
+#define CPIA2_VC_VC_HISPAN                   0xD0
+
+#define CPIA2_VC_VC_VISPAN                   0xD1
+
+#define CPIA2_VC_VC_HICROP                   0xD2
+
+#define CPIA2_VC_VC_VICROP                   0xD3
+
+#define CPIA2_VC_VC_HFRACT                   0xD4
+#define CPIA2_VC_VC_HFRACT_DEN_MASK       0x0F
+#define CPIA2_VC_VC_HFRACT_NUM_MASK       0xF0
+
+#define CPIA2_VC_VC_VFRACT                   0xD5
+#define CPIA2_VC_VC_VFRACT_DEN_MASK       0x0F
+#define CPIA2_VC_VC_VFRACT_NUM_MASK       0xF0
+
+#define CPIA2_VC_VC_JPEG_OPT                      0xD6
+#define CPIA2_VC_VC_JPEG_OPT_DOUBLE_SQUEEZE     0x01
+#define CPIA2_VC_VC_JPEG_OPT_NO_DC_AUTO_SQUEEZE 0x02
+#define CPIA2_VC_VC_JPEG_OPT_AUTO_SQUEEZE       0x04
+#define CPIA2_VC_VC_JPEG_OPT_DEFAULT      (CPIA2_VC_VC_JPEG_OPT_DOUBLE_SQUEEZE|\
+					   CPIA2_VC_VC_JPEG_OPT_AUTO_SQUEEZE)
+
+
+#define CPIA2_VC_VC_CREEP_PERIOD             0xD7
+#define CPIA2_VC_VC_USER_SQUEEZE             0xD8
+#define CPIA2_VC_VC_TARGET_KB                0xD9
+
+#define CPIA2_VC_VC_AUTO_SQUEEZE             0xE6
+
+
+/***
+ * VP register set (Bank 2)
+ ***/
+#define CPIA2_VP_DEVICEH                             0
+#define CPIA2_VP_DEVICEL                             1
+
+#define CPIA2_VP_SYSTEMSTATE                         0x02
+#define CPIA2_VP_SYSTEMSTATE_HK_ALIVE             0x01
+
+#define CPIA2_VP_SYSTEMCTRL                          0x03
+#define CPIA2_VP_SYSTEMCTRL_REQ_CLEAR_ERROR       0x80
+#define CPIA2_VP_SYSTEMCTRL_POWER_DOWN_PLL        0x20
+#define CPIA2_VP_SYSTEMCTRL_REQ_SUSPEND_STATE     0x10
+#define CPIA2_VP_SYSTEMCTRL_REQ_SERIAL_WAKEUP     0x08
+#define CPIA2_VP_SYSTEMCTRL_REQ_AUTOLOAD          0x04
+#define CPIA2_VP_SYSTEMCTRL_HK_CONTROL            0x02
+#define CPIA2_VP_SYSTEMCTRL_POWER_CONTROL         0x01
+
+#define CPIA2_VP_SENSOR_FLAGS                        0x05
+#define CPIA2_VP_SENSOR_FLAGS_404                 0x01
+#define CPIA2_VP_SENSOR_FLAGS_407                 0x02
+#define CPIA2_VP_SENSOR_FLAGS_409                 0x04
+#define CPIA2_VP_SENSOR_FLAGS_410                 0x08
+#define CPIA2_VP_SENSOR_FLAGS_500                 0x10
+
+#define CPIA2_VP_SENSOR_REV                          0x06
+
+#define CPIA2_VP_DEVICE_CONFIG                       0x07
+#define CPIA2_VP_DEVICE_CONFIG_SERIAL_BRIDGE      0x01
+
+#define CPIA2_VP_GPIO_DIRECTION                      0x08
+#define CPIA2_VP_GPIO_READ                        0xFF
+#define CPIA2_VP_GPIO_WRITE                       0x00
+
+#define CPIA2_VP_GPIO_DATA                           0x09
+
+#define CPIA2_VP_RAM_ADDR_H                          0x0A
+#define CPIA2_VP_RAM_ADDR_L                          0x0B
+#define CPIA2_VP_RAM_DATA                            0x0C
+
+#define CPIA2_VP_PATCH_REV                           0x0F
+
+#define CPIA2_VP4_USER_MODE                           0x10
+#define CPIA2_VP5_USER_MODE                           0x13
+#define CPIA2_VP_USER_MODE_CIF                    0x01
+#define CPIA2_VP_USER_MODE_QCIFDS                 0x02
+#define CPIA2_VP_USER_MODE_QCIFPTC                0x04
+#define CPIA2_VP_USER_MODE_QVGADS                 0x08
+#define CPIA2_VP_USER_MODE_QVGAPTC                0x10
+#define CPIA2_VP_USER_MODE_VGA                    0x20
+
+#define CPIA2_VP4_FRAMERATE_REQUEST                    0x11
+#define CPIA2_VP5_FRAMERATE_REQUEST                    0x14
+#define CPIA2_VP_FRAMERATE_60                     0x80
+#define CPIA2_VP_FRAMERATE_50                     0x40
+#define CPIA2_VP_FRAMERATE_30                     0x20
+#define CPIA2_VP_FRAMERATE_25                     0x10
+#define CPIA2_VP_FRAMERATE_15                     0x08
+#define CPIA2_VP_FRAMERATE_12_5                   0x04
+#define CPIA2_VP_FRAMERATE_7_5                    0x02
+#define CPIA2_VP_FRAMERATE_6_25                   0x01
+
+#define CPIA2_VP4_USER_EFFECTS                         0x12
+#define CPIA2_VP5_USER_EFFECTS                         0x15
+#define CPIA2_VP_USER_EFFECTS_COLBARS             0x01
+#define CPIA2_VP_USER_EFFECTS_COLBARS_GRAD        0x02
+#define CPIA2_VP_USER_EFFECTS_MIRROR              0x04
+#define CPIA2_VP_USER_EFFECTS_FLIP                0x40  // VP5 only
+
+/* NOTE: CPIA2_VP_EXPOSURE_MODES shares the same register as VP5 User
+ * Effects */
+#define CPIA2_VP_EXPOSURE_MODES                       0x15
+#define CPIA2_VP_EXPOSURE_MODES_INHIBIT_FLICKER   0x20
+#define CPIA2_VP_EXPOSURE_MODES_COMPILE_EXP       0x10
+
+#define CPIA2_VP4_EXPOSURE_TARGET                     0x16    // VP4
+#define CPIA2_VP5_EXPOSURE_TARGET		      0x20    // VP5
+
+#define CPIA2_VP_FLICKER_MODES                        0x1B
+#define CPIA2_VP_FLICKER_MODES_50HZ               0x80
+#define CPIA2_VP_FLICKER_MODES_CUSTOM_FLT_FFREQ   0x40
+#define CPIA2_VP_FLICKER_MODES_NEVER_FLICKER      0x20
+#define CPIA2_VP_FLICKER_MODES_INHIBIT_RUB        0x10
+#define CPIA2_VP_FLICKER_MODES_ADJUST_LINE_FREQ   0x08
+#define CPIA2_VP_FLICKER_MODES_CUSTOM_INT_FFREQ   0x04
+
+#define CPIA2_VP_UMISC                                0x1D
+#define CPIA2_VP_UMISC_FORCE_MONO                 0x80
+#define CPIA2_VP_UMISC_FORCE_ID_MASK              0x40
+#define CPIA2_VP_UMISC_INHIBIT_AUTO_FGS           0x20
+#define CPIA2_VP_UMISC_INHIBIT_AUTO_DIMS          0x08
+#define CPIA2_VP_UMISC_OPT_FOR_SENSOR_DS          0x04
+#define CPIA2_VP_UMISC_INHIBIT_AUTO_MODE_INT      0x02
+
+#define CPIA2_VP5_ANTIFLKRSETUP                       0x22  //34
+
+#define CPIA2_VP_INTERPOLATION                        0x24
+#define CPIA2_VP_INTERPOLATION_EVEN_FIRST         0x40
+#define CPIA2_VP_INTERPOLATION_HJOG               0x20
+#define CPIA2_VP_INTERPOLATION_VJOG               0x10
+
+#define CPIA2_VP_GAMMA                                0x25
+#define CPIA2_VP_DEFAULT_GAMMA                    0x10
+
+#define CPIA2_VP_YRANGE                               0x26
+
+#define CPIA2_VP_SATURATION                           0x27
+
+#define CPIA2_VP5_MYBLACK_LEVEL                       0x3A   //58
+#define CPIA2_VP5_MCYRANGE                            0x3B   //59
+#define CPIA2_VP5_MYCEILING                           0x3C   //60
+#define CPIA2_VP5_MCUVSATURATION                      0x3D   //61
+
+
+#define CPIA2_VP_REHASH_VALUES                        0x60
+
+
+/***
+ * Common sensor registers
+ ***/
+#define CPIA2_SENSOR_DEVICE_H                         0x00
+#define CPIA2_SENSOR_DEVICE_L                         0x01
+
+#define CPIA2_SENSOR_DATA_FORMAT                      0x16
+#define CPIA2_SENSOR_DATA_FORMAT_HMIRROR      0x08
+#define CPIA2_SENSOR_DATA_FORMAT_VMIRROR      0x10
+
+#define CPIA2_SENSOR_CR1                              0x76
+#define CPIA2_SENSOR_CR1_STAND_BY             0x01
+#define CPIA2_SENSOR_CR1_DOWN_RAMP_GEN        0x02
+#define CPIA2_SENSOR_CR1_DOWN_COLUMN_ADC      0x04
+#define CPIA2_SENSOR_CR1_DOWN_CAB_REGULATOR   0x08
+#define CPIA2_SENSOR_CR1_DOWN_AUDIO_REGULATOR 0x10
+#define CPIA2_SENSOR_CR1_DOWN_VRT_AMP         0x20
+#define CPIA2_SENSOR_CR1_DOWN_BAND_GAP        0x40
+
+#endif
diff --git a/drivers/media/video/cpia2/cpia2_usb.c b/drivers/media/video/cpia2/cpia2_usb.c
new file mode 100644
index 00000000000000..f4da029414936b
--- /dev/null
+++ b/drivers/media/video/cpia2/cpia2_usb.c
@@ -0,0 +1,907 @@
+/****************************************************************************
+ *
+ *  Filename: cpia2_usb.c
+ *
+ *  Copyright 2001, STMicrolectronics, Inc.
+ *      Contact:  steve.miller@st.com
+ *
+ *  Description:
+ *     This is a USB driver for CPia2 based video cameras.
+ *     The infrastructure of this driver is based on the cpia usb driver by
+ *     Jochen Scharrlach and Johannes Erdfeldt.
+ *
+ *  This program is free software; you can redistribute it and/or modify
+ *  it under the terms of the GNU General Public License as published by
+ *  the Free Software Foundation; either version 2 of the License, or
+ *  (at your option) any later version.
+ *
+ *  This program is distributed in the hope that it will be useful,
+ *  but WITHOUT ANY WARRANTY; without even the implied warranty of
+ *  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ *  GNU General Public License for more details.
+ *
+ *  You should have received a copy of the GNU General Public License
+ *  along with this program; if not, write to the Free Software
+ *  Foundation, Inc., 675 Mass Ave, Cambridge, MA 02139, USA.
+ *
+ *  Stripped of 2.4 stuff ready for main kernel submit by
+ *		Alan Cox <alan@redhat.com>
+ ****************************************************************************/
+
+#include <linux/kernel.h>
+#include <linux/slab.h>
+#include <linux/usb.h>
+
+#include "cpia2.h"
+
+static int frame_sizes[] = {
+	0,	// USBIF_CMDONLY
+	0, 	// USBIF_BULK
+	128, 	// USBIF_ISO_1
+	384, 	// USBIF_ISO_2
+	640, 	// USBIF_ISO_3
+	768, 	// USBIF_ISO_4
+	896, 	// USBIF_ISO_5
+	1023, 	// USBIF_ISO_6
+};
+
+#define FRAMES_PER_DESC    10
+#define FRAME_SIZE_PER_DESC   frame_sizes[cam->cur_alt]
+
+static void process_frame(struct camera_data *cam);
+static void cpia2_usb_complete(struct urb *urb, struct pt_regs *);
+static int cpia2_usb_probe(struct usb_interface *intf,
+			   const struct usb_device_id *id);
+static void cpia2_usb_disconnect(struct usb_interface *intf);
+
+static void free_sbufs(struct camera_data *cam);
+static void add_APPn(struct camera_data *cam);
+static void add_COM(struct camera_data *cam);
+static int submit_urbs(struct camera_data *cam);
+static int set_alternate(struct camera_data *cam, unsigned int alt);
+static int configure_transfer_mode(struct camera_data *cam, unsigned int alt);
+
+static struct usb_device_id cpia2_id_table[] = {
+	{USB_DEVICE(0x0553, 0x0100)},
+	{USB_DEVICE(0x0553, 0x0140)},
+	{USB_DEVICE(0x0553, 0x0151)},  /* STV0676 */
+	{}			/* Terminating entry */
+};
+MODULE_DEVICE_TABLE(usb, cpia2_id_table);
+
+static struct usb_driver cpia2_driver = {
+	.name		= "cpia2",
+	.probe		= cpia2_usb_probe,
+	.disconnect	= cpia2_usb_disconnect,
+	.id_table	= cpia2_id_table
+};
+
+
+/******************************************************************************
+ *
+ *  process_frame
+ *
+ *****************************************************************************/
+static void process_frame(struct camera_data *cam)
+{
+	static int frame_count = 0;
+
+	unsigned char *inbuff = cam->workbuff->data;
+
+	DBG("Processing frame #%d, current:%d\n",
+	    cam->workbuff->num, cam->curbuff->num);
+
+	if(cam->workbuff->length > cam->workbuff->max_length)
+		cam->workbuff->max_length = cam->workbuff->length;
+
+	if ((inbuff[0] == 0xFF) && (inbuff[1] == 0xD8)) {
+		frame_count++;
+	} else {
+		cam->workbuff->status = FRAME_ERROR;
+		DBG("Start of frame not found\n");
+		return;
+	}
+
+	/***
+	 * Now the output buffer should have a JPEG image in it.
+	 ***/
+	if(!cam->first_image_seen) {
+		/* Always skip the first image after streaming
+		 * starts. It is almost certainly corrupt. */
+		cam->first_image_seen = 1;
+		cam->workbuff->status = FRAME_EMPTY;
+		return;
+	}
+	if (cam->workbuff->length > 3) {
+		if(cam->mmapped &&
+		   cam->workbuff->length < cam->workbuff->max_length) {
+			/* No junk in the buffers */
+			memset(cam->workbuff->data+cam->workbuff->length,
+			       0, cam->workbuff->max_length-
+				  cam->workbuff->length);
+		}
+		cam->workbuff->max_length = cam->workbuff->length;
+		cam->workbuff->status = FRAME_READY;
+
+		if(!cam->mmapped && cam->num_frames > 2) {
+			/* During normal reading, the most recent
+			 * frame will be read.  If the current frame
+			 * hasn't started reading yet, it will never
+			 * be read, so mark it empty.  If the buffer is
+			 * mmapped, or we have few buffers, we need to
+			 * wait for the user to free the buffer.
+			 *
+			 * NOTE: This is not entirely foolproof with 3
+			 * buffers, but it would take an EXTREMELY
+			 * overloaded system to cause problems (possible
+			 * image data corruption).  Basically, it would
+			 * need to take more time to execute cpia2_read
+			 * than it would for the camera to send
+			 * cam->num_frames-2 frames before problems
+			 * could occur.
+			 */
+			cam->curbuff->status = FRAME_EMPTY;
+		}
+		cam->curbuff = cam->workbuff;
+		cam->workbuff = cam->workbuff->next;
+		DBG("Changed buffers, work:%d, current:%d\n",
+		    cam->workbuff->num, cam->curbuff->num);
+		return;
+	} else {
+		DBG("Not enough data for an image.\n");
+	}
+
+	cam->workbuff->status = FRAME_ERROR;
+	return;
+}
+
+/******************************************************************************
+ *
+ *  add_APPn
+ *
+ *  Adds a user specified APPn record
+ *****************************************************************************/
+static void add_APPn(struct camera_data *cam)
+{
+	if(cam->APP_len > 0) {
+		cam->workbuff->data[cam->workbuff->length++] = 0xFF;
+		cam->workbuff->data[cam->workbuff->length++] = 0xE0+cam->APPn;
+		cam->workbuff->data[cam->workbuff->length++] = 0;
+		cam->workbuff->data[cam->workbuff->length++] = cam->APP_len+2;
+		memcpy(cam->workbuff->data+cam->workbuff->length,
+		       cam->APP_data, cam->APP_len);
+		cam->workbuff->length += cam->APP_len;
+	}
+}
+
+/******************************************************************************
+ *
+ *  add_COM
+ *
+ *  Adds a user specified COM record
+ *****************************************************************************/
+static void add_COM(struct camera_data *cam)
+{
+	if(cam->COM_len > 0) {
+		cam->workbuff->data[cam->workbuff->length++] = 0xFF;
+		cam->workbuff->data[cam->workbuff->length++] = 0xFE;
+		cam->workbuff->data[cam->workbuff->length++] = 0;
+		cam->workbuff->data[cam->workbuff->length++] = cam->COM_len+2;
+		memcpy(cam->workbuff->data+cam->workbuff->length,
+		       cam->COM_data, cam->COM_len);
+		cam->workbuff->length += cam->COM_len;
+	}
+}
+
+/******************************************************************************
+ *
+ *  cpia2_usb_complete
+ *
+ *  callback when incoming packet is received
+ *****************************************************************************/
+static void cpia2_usb_complete(struct urb *urb, struct pt_regs *regs)
+{
+	int i;
+	unsigned char *cdata;
+	static int frame_ready = false;
+	struct camera_data *cam = (struct camera_data *) urb->context;
+
+	if (urb->status!=0) {
+		if (!(urb->status == -ENOENT ||
+		      urb->status == -ECONNRESET ||
+		      urb->status == -ESHUTDOWN))
+		{
+			DBG("urb->status = %d!\n", urb->status);
+		}
+		DBG("Stopping streaming\n");
+		return;
+	}
+
+	if (!cam->streaming || !cam->present || cam->open_count == 0) {
+		LOG("Will now stop the streaming: streaming = %d, "
+		    "present=%d, open_count=%d\n",
+		    cam->streaming, cam->present, cam->open_count);
+		return;
+	}
+
+	/***
+	 * Packet collater
+	 ***/
+	//DBG("Collating %d packets\n", urb->number_of_packets);
+	for (i = 0; i < urb->number_of_packets; i++) {
+		u16 checksum, iso_checksum;
+		int j;
+		int n = urb->iso_frame_desc[i].actual_length;
+		int st = urb->iso_frame_desc[i].status;
+
+		if(cam->workbuff->status == FRAME_READY) {
+			struct framebuf *ptr;
+			/* Try to find an available buffer */
+			DBG("workbuff full, searching\n");
+			for (ptr = cam->workbuff->next;
+			     ptr != cam->workbuff;
+			     ptr = ptr->next)
+			{
+				if (ptr->status == FRAME_EMPTY) {
+					ptr->status = FRAME_READING;
+					ptr->length = 0;
+					break;
+				}
+			}
+			if (ptr == cam->workbuff)
+				break; /* No READING or EMPTY buffers left */
+
+			cam->workbuff = ptr;
+		}
+
+		if (cam->workbuff->status == FRAME_EMPTY ||
+		    cam->workbuff->status == FRAME_ERROR) {
+			cam->workbuff->status = FRAME_READING;
+			cam->workbuff->length = 0;
+		}
+
+		//DBG("   Packet %d length = %d, status = %d\n", i, n, st);
+		cdata = urb->transfer_buffer + urb->iso_frame_desc[i].offset;
+
+		if (st) {
+			LOG("cpia2 data error: [%d] len=%d, status = %d\n",
+			    i, n, st);
+			if(!ALLOW_CORRUPT)
+				cam->workbuff->status = FRAME_ERROR;
+			continue;
+		}
+
+		if(n<=2)
+			continue;
+
+		checksum = 0;
+		for(j=0; j<n-2; ++j)
+			checksum += cdata[j];
+		iso_checksum = cdata[j] + cdata[j+1]*256;
+		if(checksum != iso_checksum) {
+			LOG("checksum mismatch: [%d] len=%d, calculated = %x, checksum = %x\n",
+			    i, n, (int)checksum, (int)iso_checksum);
+			if(!ALLOW_CORRUPT) {
+				cam->workbuff->status = FRAME_ERROR;
+				continue;
+			}
+		}
+		n -= 2;
+
+		if(cam->workbuff->status != FRAME_READING) {
+			if((0xFF == cdata[0] && 0xD8 == cdata[1]) ||
+			   (0xD8 == cdata[0] && 0xFF == cdata[1] &&
+			    0 != cdata[2])) {
+				/* frame is skipped, but increment total
+				 * frame count anyway */
+				cam->frame_count++;
+			}
+			DBG("workbuff not reading, status=%d\n",
+			    cam->workbuff->status);
+			continue;
+		}
+
+		if (cam->frame_size < cam->workbuff->length + n) {
+			ERR("buffer overflow! length: %d, n: %d\n",
+			    cam->workbuff->length, n);
+			cam->workbuff->status = FRAME_ERROR;
+			if(cam->workbuff->length > cam->workbuff->max_length)
+				cam->workbuff->max_length =
+					cam->workbuff->length;
+			continue;
+		}
+
+		if (cam->workbuff->length == 0) {
+			int data_offset;
+			if ((0xD8 == cdata[0]) && (0xFF == cdata[1])) {
+				data_offset = 1;
+			} else if((0xFF == cdata[0]) && (0xD8 == cdata[1])
+				  && (0xFF == cdata[2])) {
+				data_offset = 2;
+			} else {
+				DBG("Ignoring packet, not beginning!\n");
+				continue;
+			}
+			DBG("Start of frame pattern found\n");
+			do_gettimeofday(&cam->workbuff->timestamp);
+			cam->workbuff->seq = cam->frame_count++;
+			cam->workbuff->data[0] = 0xFF;
+			cam->workbuff->data[1] = 0xD8;
+			cam->workbuff->length = 2;
+			add_APPn(cam);
+			add_COM(cam);
+			memcpy(cam->workbuff->data+cam->workbuff->length,
+			       cdata+data_offset, n-data_offset);
+			cam->workbuff->length += n-data_offset;
+		} else if (cam->workbuff->length > 0) {
+			memcpy(cam->workbuff->data + cam->workbuff->length,
+			       cdata, n);
+			cam->workbuff->length += n;
+		}
+
+		if ((cam->workbuff->length >= 3) &&
+		    (cam->workbuff->data[cam->workbuff->length - 3] == 0xFF) &&
+		    (cam->workbuff->data[cam->workbuff->length - 2] == 0xD9) &&
+		    (cam->workbuff->data[cam->workbuff->length - 1] == 0xFF)) {
+			frame_ready = true;
+			cam->workbuff->data[cam->workbuff->length - 1] = 0;
+			cam->workbuff->length -= 1;
+		} else if ((cam->workbuff->length >= 2) &&
+		   (cam->workbuff->data[cam->workbuff->length - 2] == 0xFF) &&
+		   (cam->workbuff->data[cam->workbuff->length - 1] == 0xD9)) {
+			frame_ready = true;
+		}
+
+		if (frame_ready) {
+			DBG("Workbuff image size = %d\n",cam->workbuff->length);
+			process_frame(cam);
+
+			frame_ready = false;
+
+			if (waitqueue_active(&cam->wq_stream))
+				wake_up_interruptible(&cam->wq_stream);
+		}
+	}
+
+	if(cam->streaming) {
+		/* resubmit */
+		urb->dev = cam->dev;
+		if ((i = usb_submit_urb(urb, GFP_ATOMIC)) != 0)
+			ERR("%s: usb_submit_urb ret %d!\n", __func__, i);
+	}
+}
+
+/******************************************************************************
+ *
+ * configure_transfer_mode
+ *
+ *****************************************************************************/
+static int configure_transfer_mode(struct camera_data *cam, unsigned int alt)
+{
+	static unsigned char iso_regs[8][4] = {
+		{0x00, 0x00, 0x00, 0x00},
+		{0x00, 0x00, 0x00, 0x00},
+		{0xB9, 0x00, 0x00, 0x7E},
+		{0xB9, 0x00, 0x01, 0x7E},
+		{0xB9, 0x00, 0x02, 0x7E},
+		{0xB9, 0x00, 0x02, 0xFE},
+		{0xB9, 0x00, 0x03, 0x7E},
+		{0xB9, 0x00, 0x03, 0xFD}
+	};
+	struct cpia2_command cmd;
+	unsigned char reg;
+
+	if(!cam->present)
+		return -ENODEV;
+
+	/***
+	 * Write the isoc registers according to the alternate selected
+	 ***/
+	cmd.direction = TRANSFER_WRITE;
+	cmd.buffer.block_data[0] = iso_regs[alt][0];
+	cmd.buffer.block_data[1] = iso_regs[alt][1];
+	cmd.buffer.block_data[2] = iso_regs[alt][2];
+	cmd.buffer.block_data[3] = iso_regs[alt][3];
+	cmd.req_mode = CAMERAACCESS_TYPE_BLOCK | CAMERAACCESS_VC;
+	cmd.start = CPIA2_VC_USB_ISOLIM;
+	cmd.reg_count = 4;
+	cpia2_send_command(cam, &cmd);
+
+	/***
+	 * Enable relevant streams before starting polling.
+	 * First read USB Stream Config Register.
+	 ***/
+	cmd.direction = TRANSFER_READ;
+	cmd.req_mode = CAMERAACCESS_TYPE_BLOCK | CAMERAACCESS_VC;
+	cmd.start = CPIA2_VC_USB_STRM;
+	cmd.reg_count = 1;
+	cpia2_send_command(cam, &cmd);
+	reg = cmd.buffer.block_data[0];
+
+	/* Clear iso, bulk, and int */
+	reg &= ~(CPIA2_VC_USB_STRM_BLK_ENABLE |
+		 CPIA2_VC_USB_STRM_ISO_ENABLE |
+		 CPIA2_VC_USB_STRM_INT_ENABLE);
+
+	if (alt == USBIF_BULK) {
+		DBG("Enabling bulk xfer\n");
+		reg |= CPIA2_VC_USB_STRM_BLK_ENABLE;	/* Enable Bulk */
+		cam->xfer_mode = XFER_BULK;
+	} else if (alt >= USBIF_ISO_1) {
+		DBG("Enabling ISOC xfer\n");
+		reg |= CPIA2_VC_USB_STRM_ISO_ENABLE;
+		cam->xfer_mode = XFER_ISOC;
+	}
+
+	cmd.buffer.block_data[0] = reg;
+	cmd.direction = TRANSFER_WRITE;
+	cmd.start = CPIA2_VC_USB_STRM;
+	cmd.reg_count = 1;
+	cmd.req_mode = CAMERAACCESS_TYPE_BLOCK | CAMERAACCESS_VC;
+	cpia2_send_command(cam, &cmd);
+
+	return 0;
+}
+
+/******************************************************************************
+ *
+ * cpia2_usb_change_streaming_alternate
+ *
+ *****************************************************************************/
+int cpia2_usb_change_streaming_alternate(struct camera_data *cam,
+					 unsigned int alt)
+{
+	int ret = 0;
+
+	if(alt < USBIF_ISO_1 || alt > USBIF_ISO_6)
+		return -EINVAL;
+
+	if(alt == cam->params.camera_state.stream_mode)
+		return 0;
+
+	cpia2_usb_stream_pause(cam);
+
+	configure_transfer_mode(cam, alt);
+
+	cam->params.camera_state.stream_mode = alt;
+
+	/* Reset the camera to prevent image quality degradation */
+	cpia2_reset_camera(cam);
+
+	cpia2_usb_stream_resume(cam);
+
+	return ret;
+}
+
+/******************************************************************************
+ *
+ * set_alternate
+ *
+ *****************************************************************************/
+int set_alternate(struct camera_data *cam, unsigned int alt)
+{
+	int ret = 0;
+
+	if(alt == cam->cur_alt)
+		return 0;
+
+	if (cam->cur_alt != USBIF_CMDONLY) {
+		DBG("Changing from alt %d to %d\n", cam->cur_alt, USBIF_CMDONLY);
+		ret = usb_set_interface(cam->dev, cam->iface, USBIF_CMDONLY);
+		if (ret != 0)
+			return ret;
+	}
+	if (alt != USBIF_CMDONLY) {
+		DBG("Changing from alt %d to %d\n", USBIF_CMDONLY, alt);
+		ret = usb_set_interface(cam->dev, cam->iface, alt);
+		if (ret != 0)
+			return ret;
+	}
+
+	cam->old_alt = cam->cur_alt;
+	cam->cur_alt = alt;
+
+	return ret;
+}
+
+/******************************************************************************
+ *
+ * free_sbufs
+ *
+ * Free all cam->sbuf[]. All non-NULL .data and .urb members that are non-NULL
+ * are assumed to be allocated. Non-NULL .urb members are also assumed to be
+ * submitted (and must therefore be killed before they are freed).
+ *****************************************************************************/
+static void free_sbufs(struct camera_data *cam)
+{
+	int i;
+
+	for (i = 0; i < NUM_SBUF; i++) {
+		if(cam->sbuf[i].urb) {
+			usb_kill_urb(cam->sbuf[i].urb);
+			usb_free_urb(cam->sbuf[i].urb);
+			cam->sbuf[i].urb = NULL;
+		}
+		if(cam->sbuf[i].data) {
+			kfree(cam->sbuf[i].data);
+			cam->sbuf[i].data = NULL;
+		}
+	}
+}
+
+/*******
+* Convenience functions
+*******/
+/****************************************************************************
+ *
+ *  write_packet
+ *
+ ***************************************************************************/
+static int write_packet(struct usb_device *udev,
+			u8 request, u8 * registers, u16 start, size_t size)
+{
+	if (!registers || size <= 0)
+		return -EINVAL;
+
+	return usb_control_msg(udev,
+			       usb_sndctrlpipe(udev, 0),
+			       request,
+			       USB_TYPE_VENDOR | USB_RECIP_DEVICE,
+			       start,	/* value */
+			       0,	/* index */
+			       registers,	/* buffer */
+			       size,
+			       HZ);
+}
+
+/****************************************************************************
+ *
+ *  read_packet
+ *
+ ***************************************************************************/
+static int read_packet(struct usb_device *udev,
+		       u8 request, u8 * registers, u16 start, size_t size)
+{
+	if (!registers || size <= 0)
+		return -EINVAL;
+
+	return usb_control_msg(udev,
+			       usb_rcvctrlpipe(udev, 0),
+			       request,
+			       USB_DIR_IN|USB_TYPE_VENDOR|USB_RECIP_DEVICE,
+			       start,	/* value */
+			       0,	/* index */
+			       registers,	/* buffer */
+			       size,
+			       HZ);
+}
+
+/******************************************************************************
+ *
+ *  cpia2_usb_transfer_cmd
+ *
+ *****************************************************************************/
+int cpia2_usb_transfer_cmd(struct camera_data *cam,
+			   void *registers,
+			   u8 request, u8 start, u8 count, u8 direction)
+{
+	int err = 0;
+	struct usb_device *udev = cam->dev;
+
+	if (!udev) {
+		ERR("%s: Internal driver error: udev is NULL\n", __func__);
+		return -EINVAL;
+	}
+
+	if (!registers) {
+		ERR("%s: Internal driver error: register array is NULL\n", __func__);
+		return -EINVAL;
+	}
+
+	if (direction == TRANSFER_READ) {
+		err = read_packet(udev, request, (u8 *)registers, start, count);
+		if (err > 0)
+			err = 0;
+	} else if (direction == TRANSFER_WRITE) {
+		err =write_packet(udev, request, (u8 *)registers, start, count);
+		if (err < 0) {
+			LOG("Control message failed, err val = %d\n", err);
+			LOG("Message: request = 0x%0X, start = 0x%0X\n",
+			    request, start);
+			LOG("Message: count = %d, register[0] = 0x%0X\n",
+			    count, ((unsigned char *) registers)[0]);
+		} else
+			err=0;
+	} else {
+		LOG("Unexpected first byte of direction: %d\n",
+		       direction);
+		return -EINVAL;
+	}
+
+	if(err != 0)
+		LOG("Unexpected error: %d\n", err);
+	return err;
+}
+
+
+/******************************************************************************
+ *
+ *  submit_urbs
+ *
+ *****************************************************************************/
+static int submit_urbs(struct camera_data *cam)
+{
+	struct urb *urb;
+	int fx, err, i;
+
+	for(i=0; i<NUM_SBUF; ++i) {
+		if (cam->sbuf[i].data)
+			continue;
+		cam->sbuf[i].data =
+		    kmalloc(FRAMES_PER_DESC * FRAME_SIZE_PER_DESC, GFP_KERNEL);
+		if (!cam->sbuf[i].data) {
+			return -ENOMEM;
+		}
+	}
+
+	/* We double buffer the Isoc lists, and also know the polling
+	 * interval is every frame (1 == (1 << (bInterval -1))).
+	 */
+	for(i=0; i<NUM_SBUF; ++i) {
+		if(cam->sbuf[i].urb) {
+			continue;
+		}
+		urb = usb_alloc_urb(FRAMES_PER_DESC, GFP_KERNEL);
+		if (!urb) {
+			return -ENOMEM;
+		}
+
+		cam->sbuf[i].urb = urb;
+		urb->dev = cam->dev;
+		urb->context = cam;
+		urb->pipe = usb_rcvisocpipe(cam->dev, 1 /*ISOC endpoint*/);
+		urb->transfer_flags = URB_ISO_ASAP;
+		urb->transfer_buffer = cam->sbuf[i].data;
+		urb->complete = cpia2_usb_complete;
+		urb->number_of_packets = FRAMES_PER_DESC;
+		urb->interval = 1;
+		urb->transfer_buffer_length =
+			FRAME_SIZE_PER_DESC * FRAMES_PER_DESC;
+
+		for (fx = 0; fx < FRAMES_PER_DESC; fx++) {
+			urb->iso_frame_desc[fx].offset =
+				FRAME_SIZE_PER_DESC * fx;
+			urb->iso_frame_desc[fx].length = FRAME_SIZE_PER_DESC;
+		}
+	}
+
+
+	/* Queue the ISO urbs, and resubmit in the completion handler */
+	for(i=0; i<NUM_SBUF; ++i) {
+		err = usb_submit_urb(cam->sbuf[i].urb, GFP_KERNEL);
+		if (err) {
+			ERR("usb_submit_urb[%d]() = %d\n", i, err);
+			return err;
+		}
+	}
+
+	return 0;
+}
+
+/******************************************************************************
+ *
+ *  cpia2_usb_stream_start
+ *
+ *****************************************************************************/
+int cpia2_usb_stream_start(struct camera_data *cam, unsigned int alternate)
+{
+	int ret;
+	int old_alt;
+
+	if(cam->streaming)
+		return 0;
+
+	if (cam->flush) {
+		int i;
+		DBG("Flushing buffers\n");
+		for(i=0; i<cam->num_frames; ++i) {
+			cam->buffers[i].status = FRAME_EMPTY;
+			cam->buffers[i].length = 0;
+		}
+		cam->curbuff = &cam->buffers[0];
+		cam->workbuff = cam->curbuff->next;
+		cam->flush = false;
+	}
+
+	old_alt = cam->params.camera_state.stream_mode;
+	cam->params.camera_state.stream_mode = 0;
+	ret = cpia2_usb_change_streaming_alternate(cam, alternate);
+	if (ret < 0) {
+		int ret2;
+		ERR("cpia2_usb_change_streaming_alternate() = %d!\n", ret);
+		cam->params.camera_state.stream_mode = old_alt;
+		ret2 = set_alternate(cam, USBIF_CMDONLY);
+		if (ret2 < 0) {
+			ERR("cpia2_usb_change_streaming_alternate(%d) =%d has already "
+			    "failed. Then tried to call "
+			    "set_alternate(USBIF_CMDONLY) = %d.\n",
+			    alternate, ret, ret2);
+		}
+	} else {
+		cam->frame_count = 0;
+		cam->streaming = 1;
+		ret = cpia2_usb_stream_resume(cam);
+	}
+	return ret;
+}
+
+/******************************************************************************
+ *
+ *  cpia2_usb_stream_pause
+ *
+ *****************************************************************************/
+int cpia2_usb_stream_pause(struct camera_data *cam)
+{
+	int ret = 0;
+	if(cam->streaming) {
+		ret = set_alternate(cam, USBIF_CMDONLY);
+		free_sbufs(cam);
+	}
+	return ret;
+}
+
+/******************************************************************************
+ *
+ *  cpia2_usb_stream_resume
+ *
+ *****************************************************************************/
+int cpia2_usb_stream_resume(struct camera_data *cam)
+{
+	int ret = 0;
+	if(cam->streaming) {
+		cam->first_image_seen = 0;
+		ret = set_alternate(cam, cam->params.camera_state.stream_mode);
+		if(ret == 0) {
+			ret = submit_urbs(cam);
+		}
+	}
+	return ret;
+}
+
+/******************************************************************************
+ *
+ *  cpia2_usb_stream_stop
+ *
+ *****************************************************************************/
+int cpia2_usb_stream_stop(struct camera_data *cam)
+{
+	int ret;
+	ret = cpia2_usb_stream_pause(cam);
+	cam->streaming = 0;
+	configure_transfer_mode(cam, 0);
+	return ret;
+}
+
+/******************************************************************************
+ *
+ *  cpia2_usb_probe
+ *
+ *  Probe and initialize.
+ *****************************************************************************/
+static int cpia2_usb_probe(struct usb_interface *intf,
+			   const struct usb_device_id *id)
+{
+	struct usb_device *udev = interface_to_usbdev(intf);
+	struct usb_interface_descriptor *interface;
+	struct camera_data *cam;
+	int ret;
+
+	/* A multi-config CPiA2 camera? */
+	if (udev->descriptor.bNumConfigurations != 1)
+		return -ENODEV;
+	interface = &intf->cur_altsetting->desc;
+
+	/* If we get to this point, we found a CPiA2 camera */
+	LOG("CPiA2 USB camera found\n");
+
+	if((cam = cpia2_init_camera_struct()) == NULL)
+		return -ENOMEM;
+
+	cam->dev = udev;
+	cam->iface = interface->bInterfaceNumber;
+
+	ret = set_alternate(cam, USBIF_CMDONLY);
+	if (ret < 0) {
+		ERR("%s: usb_set_interface error (ret = %d)\n", __func__, ret);
+		kfree(cam);
+		return ret;
+	}
+
+	if ((ret = cpia2_register_camera(cam)) < 0) {
+		ERR("%s: Failed to register cpia2 camera (ret = %d)\n", __func__, ret);
+		kfree(cam);
+		return ret;
+	}
+
+
+	if((ret = cpia2_init_camera(cam)) < 0) {
+		ERR("%s: failed to initialize cpia2 camera (ret = %d)\n", __func__, ret);
+		cpia2_unregister_camera(cam);
+		kfree(cam);
+		return ret;
+	}
+	LOG("  CPiA Version: %d.%02d (%d.%d)\n",
+	       cam->params.version.firmware_revision_hi,
+	       cam->params.version.firmware_revision_lo,
+	       cam->params.version.asic_id,
+	       cam->params.version.asic_rev);
+	LOG("  CPiA PnP-ID: %04x:%04x:%04x\n",
+	       cam->params.pnp_id.vendor,
+	       cam->params.pnp_id.product,
+	       cam->params.pnp_id.device_revision);
+	LOG("  SensorID: %d.(version %d)\n",
+	       cam->params.version.sensor_flags,
+	       cam->params.version.sensor_rev);
+
+	usb_set_intfdata(intf, cam);
+
+	return 0;
+}
+
+/******************************************************************************
+ *
+ *  cpia2_disconnect
+ *
+ *****************************************************************************/
+static void cpia2_usb_disconnect(struct usb_interface *intf)
+{
+	struct camera_data *cam = usb_get_intfdata(intf);
+	usb_set_intfdata(intf, NULL);
+	cam->present = 0;
+
+	DBG("Stopping stream\n");
+	cpia2_usb_stream_stop(cam);
+
+	DBG("Unregistering camera\n");
+	cpia2_unregister_camera(cam);
+
+	if(cam->buffers) {
+		DBG("Wakeup waiting processes\n");
+		cam->curbuff->status = FRAME_READY;
+		cam->curbuff->length = 0;
+		if (waitqueue_active(&cam->wq_stream))
+			wake_up_interruptible(&cam->wq_stream);
+	}
+
+	DBG("Releasing interface\n");
+	usb_driver_release_interface(&cpia2_driver, intf);
+
+	if (cam->open_count == 0) {
+		DBG("Freeing camera structure\n");
+		kfree(cam);
+	}
+
+	LOG("CPiA2 camera disconnected.\n");
+}
+
+
+/******************************************************************************
+ *
+ *  usb_cpia2_init
+ *
+ *****************************************************************************/
+int cpia2_usb_init(void)
+{
+	return usb_register(&cpia2_driver);
+}
+
+/******************************************************************************
+ *
+ *  usb_cpia_cleanup
+ *
+ *****************************************************************************/
+void cpia2_usb_cleanup(void)
+{
+	schedule_timeout(2 * HZ);
+	usb_deregister(&cpia2_driver);
+}
diff --git a/drivers/media/video/cpia2/cpia2_v4l.c b/drivers/media/video/cpia2/cpia2_v4l.c
new file mode 100644
index 00000000000000..3480a2ca7cdc60
--- /dev/null
+++ b/drivers/media/video/cpia2/cpia2_v4l.c
@@ -0,0 +1,2104 @@
+/****************************************************************************
+ *
+ *  Filename: cpia2_v4l.c
+ *
+ *  Copyright 2001, STMicrolectronics, Inc.
+ *      Contact:  steve.miller@st.com
+ *  Copyright 2001,2005, Scott J. Bertin <scottbertin@yahoo.com>
+ *
+ *  Description:
+ *     This is a USB driver for CPia2 based video cameras.
+ *     The infrastructure of this driver is based on the cpia usb driver by
+ *     Jochen Scharrlach and Johannes Erdfeldt.
+ *
+ *  This program is free software; you can redistribute it and/or modify
+ *  it under the terms of the GNU General Public License as published by
+ *  the Free Software Foundation; either version 2 of the License, or
+ *  (at your option) any later version.
+ *
+ *  This program is distributed in the hope that it will be useful,
+ *  but WITHOUT ANY WARRANTY; without even the implied warranty of
+ *  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ *  GNU General Public License for more details.
+ *
+ *  You should have received a copy of the GNU General Public License
+ *  along with this program; if not, write to the Free Software
+ *  Foundation, Inc., 675 Mass Ave, Cambridge, MA 02139, USA.
+ *
+ *  Stripped of 2.4 stuff ready for main kernel submit by
+ *		Alan Cox <alan@redhat.com>
+ ****************************************************************************/
+
+#include <linux/version.h>
+
+#include <linux/config.h>
+
+#include <linux/module.h>
+#include <linux/time.h>
+#include <linux/sched.h>
+#include <linux/slab.h>
+#include <linux/init.h>
+#include <linux/moduleparam.h>
+
+#include "cpia2.h"
+#include "cpia2dev.h"
+
+
+//#define _CPIA2_DEBUG_
+
+#define MAKE_STRING_1(x)	#x
+#define MAKE_STRING(x)	MAKE_STRING_1(x)
+
+static int video_nr = -1;
+module_param(video_nr, int, 0);
+MODULE_PARM_DESC(video_nr,"video device to register (0=/dev/video0, etc)");
+
+static int buffer_size = 68*1024;
+module_param(buffer_size, int, 0);
+MODULE_PARM_DESC(buffer_size, "Size for each frame buffer in bytes (default 68k)");
+
+static int num_buffers = 3;
+module_param(num_buffers, int, 0);
+MODULE_PARM_DESC(num_buffers, "Number of frame buffers (1-"
+		 MAKE_STRING(VIDEO_MAX_FRAME) ", default 3)");
+
+static int alternate = DEFAULT_ALT;
+module_param(alternate, int, 0);
+MODULE_PARM_DESC(alternate, "USB Alternate (" MAKE_STRING(USBIF_ISO_1) "-"
+		 MAKE_STRING(USBIF_ISO_6) ", default "
+		 MAKE_STRING(DEFAULT_ALT) ")");
+
+static int flicker_freq = 60;
+module_param(flicker_freq, int, 0);
+MODULE_PARM_DESC(flicker_freq, "Flicker frequency (" MAKE_STRING(50) "or"
+		 MAKE_STRING(60) ", default "
+		 MAKE_STRING(60) ")");
+
+static int flicker_mode = NEVER_FLICKER;
+module_param(flicker_mode, int, 0);
+MODULE_PARM_DESC(flicker_mode,
+		 "Flicker supression (" MAKE_STRING(NEVER_FLICKER) "or"
+		 MAKE_STRING(ANTI_FLICKER_ON) ", default "
+		 MAKE_STRING(NEVER_FLICKER) ")");
+
+MODULE_AUTHOR("Steve Miller (STMicroelectronics) <steve.miller@st.com>");
+MODULE_DESCRIPTION("V4L-driver for STMicroelectronics CPiA2 based cameras");
+MODULE_SUPPORTED_DEVICE("video");
+MODULE_LICENSE("GPL");
+
+#define ABOUT "V4L-Driver for Vision CPiA2 based cameras"
+
+#ifndef VID_HARDWARE_CPIA2
+#error "VID_HARDWARE_CPIA2 should have been defined in linux/videodev.h"
+#endif
+
+struct control_menu_info {
+	int value;
+	char name[32];
+};
+
+static struct control_menu_info framerate_controls[] =
+{
+	{ CPIA2_VP_FRAMERATE_6_25, "6.25 fps" },
+	{ CPIA2_VP_FRAMERATE_7_5,  "7.5 fps"  },
+	{ CPIA2_VP_FRAMERATE_12_5, "12.5 fps" },
+	{ CPIA2_VP_FRAMERATE_15,   "15 fps"   },
+	{ CPIA2_VP_FRAMERATE_25,   "25 fps"   },
+	{ CPIA2_VP_FRAMERATE_30,   "30 fps"   },
+};
+#define NUM_FRAMERATE_CONTROLS (sizeof(framerate_controls)/sizeof(framerate_controls[0]))
+
+static struct control_menu_info flicker_controls[] =
+{
+	{ NEVER_FLICKER, "Off" },
+	{ FLICKER_50,    "50 Hz" },
+	{ FLICKER_60,    "60 Hz"  },
+};
+#define NUM_FLICKER_CONTROLS (sizeof(flicker_controls)/sizeof(flicker_controls[0]))
+
+static struct control_menu_info lights_controls[] =
+{
+	{ 0,   "Off" },
+	{ 64,  "Top" },
+	{ 128, "Bottom"  },
+	{ 192, "Both"  },
+};
+#define NUM_LIGHTS_CONTROLS (sizeof(lights_controls)/sizeof(lights_controls[0]))
+#define GPIO_LIGHTS_MASK 192
+
+static struct v4l2_queryctrl controls[] = {
+	{
+		.id            = V4L2_CID_BRIGHTNESS,
+		.type          = V4L2_CTRL_TYPE_INTEGER,
+		.name          = "Brightness",
+		.minimum       = 0,
+		.maximum       = 255,
+		.step          = 1,
+		.default_value = DEFAULT_BRIGHTNESS,
+	},
+	{
+		.id            = V4L2_CID_CONTRAST,
+		.type          = V4L2_CTRL_TYPE_INTEGER,
+		.name          = "Contrast",
+		.minimum       = 0,
+		.maximum       = 255,
+		.step          = 1,
+		.default_value = DEFAULT_CONTRAST,
+	},
+	{
+		.id            = V4L2_CID_SATURATION,
+		.type          = V4L2_CTRL_TYPE_INTEGER,
+		.name          = "Saturation",
+		.minimum       = 0,
+		.maximum       = 255,
+		.step          = 1,
+		.default_value = DEFAULT_SATURATION,
+	},
+	{
+		.id            = V4L2_CID_HFLIP,
+		.type          = V4L2_CTRL_TYPE_BOOLEAN,
+		.name          = "Mirror Horizontally",
+		.minimum       = 0,
+		.maximum       = 1,
+		.step          = 1,
+		.default_value = 0,
+	},
+	{
+		.id            = V4L2_CID_VFLIP,
+		.type          = V4L2_CTRL_TYPE_BOOLEAN,
+		.name          = "Flip Vertically",
+		.minimum       = 0,
+		.maximum       = 1,
+		.step          = 1,
+		.default_value = 0,
+	},
+	{
+		.id            = CPIA2_CID_TARGET_KB,
+		.type          = V4L2_CTRL_TYPE_INTEGER,
+		.name          = "Target KB",
+		.minimum       = 0,
+		.maximum       = 255,
+		.step          = 1,
+		.default_value = DEFAULT_TARGET_KB,
+	},
+	{
+		.id            = CPIA2_CID_GPIO,
+		.type          = V4L2_CTRL_TYPE_INTEGER,
+		.name          = "GPIO",
+		.minimum       = 0,
+		.maximum       = 255,
+		.step          = 1,
+		.default_value = 0,
+	},
+	{
+		.id            = CPIA2_CID_FLICKER_MODE,
+		.type          = V4L2_CTRL_TYPE_MENU,
+		.name          = "Flicker Reduction",
+		.minimum       = 0,
+		.maximum       = NUM_FLICKER_CONTROLS-1,
+		.step          = 1,
+		.default_value = 0,
+	},
+	{
+		.id            = CPIA2_CID_FRAMERATE,
+		.type          = V4L2_CTRL_TYPE_MENU,
+		.name          = "Framerate",
+		.minimum       = 0,
+		.maximum       = NUM_FRAMERATE_CONTROLS-1,
+		.step          = 1,
+		.default_value = NUM_FRAMERATE_CONTROLS-1,
+	},
+	{
+		.id            = CPIA2_CID_USB_ALT,
+		.type          = V4L2_CTRL_TYPE_INTEGER,
+		.name          = "USB Alternate",
+		.minimum       = USBIF_ISO_1,
+		.maximum       = USBIF_ISO_6,
+		.step          = 1,
+		.default_value = DEFAULT_ALT,
+	},
+	{
+		.id            = CPIA2_CID_LIGHTS,
+		.type          = V4L2_CTRL_TYPE_MENU,
+		.name          = "Lights",
+		.minimum       = 0,
+		.maximum       = NUM_LIGHTS_CONTROLS-1,
+		.step          = 1,
+		.default_value = 0,
+	},
+	{
+		.id            = CPIA2_CID_RESET_CAMERA,
+		.type          = V4L2_CTRL_TYPE_BUTTON,
+		.name          = "Reset Camera",
+		.minimum       = 0,
+		.maximum       = 0,
+		.step          = 0,
+		.default_value = 0,
+	},
+};
+#define NUM_CONTROLS (sizeof(controls)/sizeof(controls[0]))
+
+
+/******************************************************************************
+ *
+ *  cpia2_open
+ *
+ *****************************************************************************/
+static int cpia2_open(struct inode *inode, struct file *file)
+{
+	struct video_device *dev = video_devdata(file);
+	struct camera_data *cam = video_get_drvdata(dev);
+	int retval = 0;
+
+	if (!cam) {
+		ERR("Internal error, camera_data not found!\n");
+		return -ENODEV;
+	}
+
+	if(down_interruptible(&cam->busy_lock))
+		return -ERESTARTSYS;
+
+	if(!cam->present) {
+		retval = -ENODEV;
+		goto err_return;
+	}
+
+	if (cam->open_count > 0) {
+		goto skip_init;
+	}
+
+	if (cpia2_allocate_buffers(cam)) {
+		retval = -ENOMEM;
+		goto err_return;
+	}
+
+	/* reset the camera */
+	if (cpia2_reset_camera(cam) < 0) {
+		retval = -EIO;
+		goto err_return;
+	}
+
+	cam->APP_len = 0;
+	cam->COM_len = 0;
+
+skip_init:
+	{
+		struct cpia2_fh *fh = kmalloc(sizeof(*fh),GFP_KERNEL);
+		if(!fh) {
+			retval = -ENOMEM;
+			goto err_return;
+		}
+		file->private_data = fh;
+		fh->prio = V4L2_PRIORITY_UNSET;
+		v4l2_prio_open(&cam->prio, &fh->prio);
+		fh->mmapped = 0;
+	}
+
+	++cam->open_count;
+
+	cpia2_dbg_dump_registers(cam);
+
+err_return:
+	up(&cam->busy_lock);
+	return retval;
+}
+
+/******************************************************************************
+ *
+ *  cpia2_close
+ *
+ *****************************************************************************/
+static int cpia2_close(struct inode *inode, struct file *file)
+{
+	struct video_device *dev = video_devdata(file);
+	struct camera_data *cam = video_get_drvdata(dev);
+	struct cpia2_fh *fh = file->private_data;
+
+	down(&cam->busy_lock);
+
+	if (cam->present &&
+	    (cam->open_count == 1
+	     || fh->prio == V4L2_PRIORITY_RECORD
+	    )) {
+		cpia2_usb_stream_stop(cam);
+
+		if(cam->open_count == 1) {
+			/* save camera state for later open */
+			cpia2_save_camera_state(cam);
+
+			cpia2_set_low_power(cam);
+			cpia2_free_buffers(cam);
+		}
+	}
+
+	{
+		if(fh->mmapped)
+			cam->mmapped = 0;
+		v4l2_prio_close(&cam->prio,&fh->prio);
+		file->private_data = NULL;
+		kfree(fh);
+	}
+
+	if (--cam->open_count == 0) {
+		cpia2_free_buffers(cam);
+		if (!cam->present) {
+			video_unregister_device(dev);
+			kfree(cam);
+		}
+	}
+
+	up(&cam->busy_lock);
+
+	return 0;
+}
+
+/******************************************************************************
+ *
+ *  cpia2_v4l_read
+ *
+ *****************************************************************************/
+static ssize_t cpia2_v4l_read(struct file *file, char __user *buf, size_t count,
+			      loff_t *off)
+{
+	struct video_device *dev = video_devdata(file);
+	struct camera_data *cam = video_get_drvdata(dev);
+	int noblock = file->f_flags&O_NONBLOCK;
+
+	struct cpia2_fh *fh = file->private_data;
+
+	if(!cam)
+		return -EINVAL;
+
+	/* Priority check */
+	if(fh->prio != V4L2_PRIORITY_RECORD) {
+		return -EBUSY;
+	}
+
+	return cpia2_read(cam, buf, count, noblock);
+}
+
+
+/******************************************************************************
+ *
+ *  cpia2_v4l_poll
+ *
+ *****************************************************************************/
+static unsigned int cpia2_v4l_poll(struct file *filp, struct poll_table_struct *wait)
+{
+	struct video_device *dev = video_devdata(filp);
+	struct camera_data *cam = video_get_drvdata(dev);
+
+	struct cpia2_fh *fh = filp->private_data;
+
+	if(!cam)
+		return POLLERR;
+
+	/* Priority check */
+	if(fh->prio != V4L2_PRIORITY_RECORD) {
+		return POLLERR;
+	}
+
+	return cpia2_poll(cam, filp, wait);
+}
+
+
+/******************************************************************************
+ *
+ *  ioctl_cap_query
+ *
+ *****************************************************************************/
+static int ioctl_cap_query(void *arg, struct camera_data *cam)
+{
+	struct video_capability *vc;
+	int retval = 0;
+	vc = arg;
+
+	if (cam->params.pnp_id.product == 0x151)
+		strcpy(vc->name, "QX5 Microscope");
+	else
+		strcpy(vc->name, "CPiA2 Camera");
+
+	vc->type = VID_TYPE_CAPTURE | VID_TYPE_MJPEG_ENCODER;
+	vc->channels = 1;
+	vc->audios = 0;
+	vc->minwidth = 176;	/* VIDEOSIZE_QCIF */
+	vc->minheight = 144;
+	switch (cam->params.version.sensor_flags) {
+	case CPIA2_VP_SENSOR_FLAGS_500:
+		vc->maxwidth = STV_IMAGE_VGA_COLS;
+		vc->maxheight = STV_IMAGE_VGA_ROWS;
+		break;
+	case CPIA2_VP_SENSOR_FLAGS_410:
+		vc->maxwidth = STV_IMAGE_CIF_COLS;
+		vc->maxheight = STV_IMAGE_CIF_ROWS;
+		break;
+	default:
+		return -EINVAL;
+	}
+
+	return retval;
+}
+
+/******************************************************************************
+ *
+ *  ioctl_get_channel
+ *
+ *****************************************************************************/
+static int ioctl_get_channel(void *arg)
+{
+	int retval = 0;
+	struct video_channel *v;
+	v = arg;
+
+	if (v->channel != 0)
+		return -EINVAL;
+
+	v->channel = 0;
+	strcpy(v->name, "Camera");
+	v->tuners = 0;
+	v->flags = 0;
+	v->type = VIDEO_TYPE_CAMERA;
+	v->norm = 0;
+
+	return retval;
+}
+
+/******************************************************************************
+ *
+ *  ioctl_set_channel
+ *
+ *****************************************************************************/
+static int ioctl_set_channel(void *arg)
+{
+	struct video_channel *v;
+	int retval = 0;
+	v = arg;
+
+	if (retval == 0 && v->channel != 0)
+		retval = -EINVAL;
+
+	return retval;
+}
+
+/******************************************************************************
+ *
+ *  ioctl_set_image_prop
+ *
+ *****************************************************************************/
+static int ioctl_set_image_prop(void *arg, struct camera_data *cam)
+{
+	struct video_picture *vp;
+	int retval = 0;
+	vp = arg;
+
+	/* brightness, color, contrast need no check 0-65535 */
+	memcpy(&cam->vp, vp, sizeof(*vp));
+
+	/* update cam->params.colorParams */
+	cam->params.color_params.brightness = vp->brightness / 256;
+	cam->params.color_params.saturation = vp->colour / 256;
+	cam->params.color_params.contrast = vp->contrast / 256;
+
+	DBG("Requested params: bright 0x%X, sat 0x%X, contrast 0x%X\n",
+	    cam->params.color_params.brightness,
+	    cam->params.color_params.saturation,
+	    cam->params.color_params.contrast);
+
+	cpia2_set_color_params(cam);
+
+	return retval;
+}
+
+static int sync(struct camera_data *cam, int frame_nr)
+{
+	struct framebuf *frame = &cam->buffers[frame_nr];
+
+	while (1) {
+		if (frame->status == FRAME_READY)
+			return 0;
+
+		if (!cam->streaming) {
+			frame->status = FRAME_READY;
+			frame->length = 0;
+			return 0;
+		}
+
+		up(&cam->busy_lock);
+		wait_event_interruptible(cam->wq_stream,
+					 !cam->streaming ||
+					 frame->status == FRAME_READY);
+		down(&cam->busy_lock);
+		if (signal_pending(current))
+			return -ERESTARTSYS;
+		if(!cam->present)
+			return -ENOTTY;
+	}
+}
+
+/******************************************************************************
+ *
+ *  ioctl_set_window_size
+ *
+ *****************************************************************************/
+static int ioctl_set_window_size(void *arg, struct camera_data *cam,
+				 struct cpia2_fh *fh)
+{
+	/* copy_from_user, check validity, copy to internal structure */
+	struct video_window *vw;
+	int frame, err;
+	vw = arg;
+
+	if (vw->clipcount != 0)	/* clipping not supported */
+		return -EINVAL;
+
+	if (vw->clips != NULL)	/* clipping not supported */
+		return -EINVAL;
+
+	/* Ensure that only this process can change the format. */
+	err = v4l2_prio_change(&cam->prio, &fh->prio, V4L2_PRIORITY_RECORD);
+	if(err != 0)
+		return err;
+
+	cam->pixelformat = V4L2_PIX_FMT_JPEG;
+
+	/* Be sure to supply the Huffman tables, this isn't MJPEG */
+	cam->params.compression.inhibit_htables = 0;
+
+	/* we set the video window to something smaller or equal to what
+	 * is requested by the user???
+	 */
+	DBG("Requested width = %d, height = %d\n", vw->width, vw->height);
+	if (vw->width != cam->vw.width || vw->height != cam->vw.height) {
+		cam->vw.width = vw->width;
+		cam->vw.height = vw->height;
+		cam->params.roi.width = vw->width;
+		cam->params.roi.height = vw->height;
+		cpia2_set_format(cam);
+	}
+
+	for (frame = 0; frame < cam->num_frames; ++frame) {
+		if (cam->buffers[frame].status == FRAME_READING)
+			if ((err = sync(cam, frame)) < 0)
+				return err;
+
+		cam->buffers[frame].status = FRAME_EMPTY;
+	}
+
+	return 0;
+}
+
+/******************************************************************************
+ *
+ *  ioctl_get_mbuf
+ *
+ *****************************************************************************/
+static int ioctl_get_mbuf(void *arg, struct camera_data *cam)
+{
+	struct video_mbuf *vm;
+	int i;
+	vm = arg;
+
+	memset(vm, 0, sizeof(*vm));
+	vm->size = cam->frame_size*cam->num_frames;
+	vm->frames = cam->num_frames;
+	for (i = 0; i < cam->num_frames; i++)
+		vm->offsets[i] = cam->frame_size * i;
+
+	return 0;
+}
+
+/******************************************************************************
+ *
+ *  ioctl_mcapture
+ *
+ *****************************************************************************/
+static int ioctl_mcapture(void *arg, struct camera_data *cam,
+			  struct cpia2_fh *fh)
+{
+	struct video_mmap *vm;
+	int video_size, err;
+	vm = arg;
+
+	if (vm->frame < 0 || vm->frame >= cam->num_frames)
+		return -EINVAL;
+
+	/* set video size */
+	video_size = cpia2_match_video_size(vm->width, vm->height);
+	if (cam->video_size < 0) {
+		return -EINVAL;
+	}
+
+	/* Ensure that only this process can change the format. */
+	err = v4l2_prio_change(&cam->prio, &fh->prio, V4L2_PRIORITY_RECORD);
+	if(err != 0)
+		return err;
+
+	if (video_size != cam->video_size) {
+		cam->video_size = video_size;
+		cam->params.roi.width = vm->width;
+		cam->params.roi.height = vm->height;
+		cpia2_set_format(cam);
+	}
+
+	if (cam->buffers[vm->frame].status == FRAME_READING)
+		if ((err=sync(cam, vm->frame)) < 0)
+			return err;
+
+	cam->buffers[vm->frame].status = FRAME_EMPTY;
+
+	return cpia2_usb_stream_start(cam,cam->params.camera_state.stream_mode);
+}
+
+/******************************************************************************
+ *
+ *  ioctl_sync
+ *
+ *****************************************************************************/
+static int ioctl_sync(void *arg, struct camera_data *cam)
+{
+	int frame;
+
+	frame = *(int*)arg;
+
+	if (frame < 0 || frame >= cam->num_frames)
+		return -EINVAL;
+
+	return sync(cam, frame);
+}
+
+
+/******************************************************************************
+ *
+ *  ioctl_set_gpio
+ *
+ *****************************************************************************/
+
+static int ioctl_set_gpio(void *arg, struct camera_data *cam)
+{
+	__u32 gpio_val;
+
+	gpio_val = *(__u32*) arg;
+
+	if (gpio_val &~ 0xFFU)
+		return -EINVAL;
+
+	return cpia2_set_gpio(cam, (unsigned char)gpio_val);
+}
+
+/******************************************************************************
+ *
+ *  ioctl_querycap
+ *
+ *  V4L2 device capabilities
+ *
+ *****************************************************************************/
+
+static int ioctl_querycap(void *arg, struct camera_data *cam)
+{
+	struct v4l2_capability *vc = arg;
+
+	memset(vc, 0, sizeof(*vc));
+	strcpy(vc->driver, "cpia2");
+
+	if (cam->params.pnp_id.product == 0x151)
+		strcpy(vc->card, "QX5 Microscope");
+	else
+		strcpy(vc->card, "CPiA2 Camera");
+	switch (cam->params.pnp_id.device_type) {
+	case DEVICE_STV_672:
+		strcat(vc->card, " (672/");
+		break;
+	case DEVICE_STV_676:
+		strcat(vc->card, " (676/");
+		break;
+	default:
+		strcat(vc->card, " (???/");
+		break;
+	}
+	switch (cam->params.version.sensor_flags) {
+	case CPIA2_VP_SENSOR_FLAGS_404:
+		strcat(vc->card, "404)");
+		break;
+	case CPIA2_VP_SENSOR_FLAGS_407:
+		strcat(vc->card, "407)");
+		break;
+	case CPIA2_VP_SENSOR_FLAGS_409:
+		strcat(vc->card, "409)");
+		break;
+	case CPIA2_VP_SENSOR_FLAGS_410:
+		strcat(vc->card, "410)");
+		break;
+	case CPIA2_VP_SENSOR_FLAGS_500:
+		strcat(vc->card, "500)");
+		break;
+	default:
+		strcat(vc->card, "???)");
+		break;
+	}
+
+	if (usb_make_path(cam->dev, vc->bus_info, sizeof(vc->bus_info)) <0)
+		memset(vc->bus_info,0, sizeof(vc->bus_info));
+
+	vc->version = KERNEL_VERSION(CPIA2_MAJ_VER, CPIA2_MIN_VER,
+				     CPIA2_PATCH_VER);
+
+	vc->capabilities = V4L2_CAP_VIDEO_CAPTURE |
+			   V4L2_CAP_READWRITE |
+			   V4L2_CAP_STREAMING;
+
+	return 0;
+}
+
+/******************************************************************************
+ *
+ *  ioctl_input
+ *
+ *  V4L2 input get/set/enumerate
+ *
+ *****************************************************************************/
+
+static int ioctl_input(unsigned int ioclt_nr,void *arg,struct camera_data *cam)
+{
+	struct v4l2_input *i = arg;
+
+	if(ioclt_nr  != VIDIOC_G_INPUT) {
+		if (i->index != 0)
+		       return -EINVAL;
+	}
+
+	memset(i, 0, sizeof(*i));
+	strcpy(i->name, "Camera");
+	i->type = V4L2_INPUT_TYPE_CAMERA;
+
+	return 0;
+}
+
+/******************************************************************************
+ *
+ *  ioctl_enum_fmt
+ *
+ *  V4L2 format enumerate
+ *
+ *****************************************************************************/
+
+static int ioctl_enum_fmt(void *arg,struct camera_data *cam)
+{
+	struct v4l2_fmtdesc *f = arg;
+	int index = f->index;
+
+	if (index < 0 || index > 1)
+	       return -EINVAL;
+
+	memset(f, 0, sizeof(*f));
+	f->index = index;
+	f->type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
+	f->flags = V4L2_FMT_FLAG_COMPRESSED;
+	switch(index) {
+	case 0:
+		strcpy(f->description, "MJPEG");
+		f->pixelformat = V4L2_PIX_FMT_MJPEG;
+		break;
+	case 1:
+		strcpy(f->description, "JPEG");
+		f->pixelformat = V4L2_PIX_FMT_JPEG;
+		break;
+	default:
+		return -EINVAL;
+	}
+
+	return 0;
+}
+
+/******************************************************************************
+ *
+ *  ioctl_try_fmt
+ *
+ *  V4L2 format try
+ *
+ *****************************************************************************/
+
+static int ioctl_try_fmt(void *arg,struct camera_data *cam)
+{
+	struct v4l2_format *f = arg;
+
+	if (f->type != V4L2_BUF_TYPE_VIDEO_CAPTURE)
+	       return -EINVAL;
+
+	if (f->fmt.pix.pixelformat != V4L2_PIX_FMT_MJPEG &&
+	    f->fmt.pix.pixelformat != V4L2_PIX_FMT_JPEG)
+	       return -EINVAL;
+
+	f->fmt.pix.field = V4L2_FIELD_NONE;
+	f->fmt.pix.bytesperline = 0;
+	f->fmt.pix.sizeimage = cam->frame_size;
+	f->fmt.pix.colorspace = V4L2_COLORSPACE_JPEG;
+	f->fmt.pix.priv = 0;
+
+	switch (cpia2_match_video_size(f->fmt.pix.width, f->fmt.pix.height)) {
+	case VIDEOSIZE_VGA:
+		f->fmt.pix.width = 640;
+		f->fmt.pix.height = 480;
+		break;
+	case VIDEOSIZE_CIF:
+		f->fmt.pix.width = 352;
+		f->fmt.pix.height = 288;
+		break;
+	case VIDEOSIZE_QVGA:
+		f->fmt.pix.width = 320;
+		f->fmt.pix.height = 240;
+		break;
+	case VIDEOSIZE_288_216:
+		f->fmt.pix.width = 288;
+		f->fmt.pix.height = 216;
+		break;
+	case VIDEOSIZE_256_192:
+		f->fmt.pix.width = 256;
+		f->fmt.pix.height = 192;
+		break;
+	case VIDEOSIZE_224_168:
+		f->fmt.pix.width = 224;
+		f->fmt.pix.height = 168;
+		break;
+	case VIDEOSIZE_192_144:
+		f->fmt.pix.width = 192;
+		f->fmt.pix.height = 144;
+		break;
+	case VIDEOSIZE_QCIF:
+	default:
+		f->fmt.pix.width = 176;
+		f->fmt.pix.height = 144;
+		break;
+	}
+
+	return 0;
+}
+
+/******************************************************************************
+ *
+ *  ioctl_set_fmt
+ *
+ *  V4L2 format set
+ *
+ *****************************************************************************/
+
+static int ioctl_set_fmt(void *arg,struct camera_data *cam, struct cpia2_fh *fh)
+{
+	struct v4l2_format *f = arg;
+	int err, frame;
+
+	err = ioctl_try_fmt(arg, cam);
+	if(err != 0)
+		return err;
+
+	/* Ensure that only this process can change the format. */
+	err = v4l2_prio_change(&cam->prio, &fh->prio, V4L2_PRIORITY_RECORD);
+	if(err != 0) {
+		return err;
+	}
+
+	cam->pixelformat = f->fmt.pix.pixelformat;
+
+	/* NOTE: This should be set to 1 for MJPEG, but some apps don't handle
+	 * the missing Huffman table properly. */
+	cam->params.compression.inhibit_htables = 0;
+		/*f->fmt.pix.pixelformat == V4L2_PIX_FMT_MJPEG;*/
+
+	/* we set the video window to something smaller or equal to what
+	 * is requested by the user???
+	 */
+	DBG("Requested width = %d, height = %d\n",
+	    f->fmt.pix.width, f->fmt.pix.height);
+	if (f->fmt.pix.width != cam->vw.width ||
+	    f->fmt.pix.height != cam->vw.height) {
+		cam->vw.width = f->fmt.pix.width;
+		cam->vw.height = f->fmt.pix.height;
+		cam->params.roi.width = f->fmt.pix.width;
+		cam->params.roi.height = f->fmt.pix.height;
+		cpia2_set_format(cam);
+	}
+
+	for (frame = 0; frame < cam->num_frames; ++frame) {
+		if (cam->buffers[frame].status == FRAME_READING)
+			if ((err = sync(cam, frame)) < 0)
+				return err;
+
+		cam->buffers[frame].status = FRAME_EMPTY;
+	}
+
+	return 0;
+}
+
+/******************************************************************************
+ *
+ *  ioctl_get_fmt
+ *
+ *  V4L2 format get
+ *
+ *****************************************************************************/
+
+static int ioctl_get_fmt(void *arg,struct camera_data *cam)
+{
+	struct v4l2_format *f = arg;
+
+	if (f->type != V4L2_BUF_TYPE_VIDEO_CAPTURE)
+	       return -EINVAL;
+
+	f->fmt.pix.width = cam->vw.width;
+	f->fmt.pix.height = cam->vw.height;
+	f->fmt.pix.pixelformat = cam->pixelformat;
+	f->fmt.pix.field = V4L2_FIELD_NONE;
+	f->fmt.pix.bytesperline = 0;
+	f->fmt.pix.sizeimage = cam->frame_size;
+	f->fmt.pix.colorspace = V4L2_COLORSPACE_JPEG;
+	f->fmt.pix.priv = 0;
+
+	return 0;
+}
+
+/******************************************************************************
+ *
+ *  ioctl_cropcap
+ *
+ *  V4L2 query cropping capabilities
+ *  NOTE: cropping is currently disabled
+ *
+ *****************************************************************************/
+
+static int ioctl_cropcap(void *arg,struct camera_data *cam)
+{
+	struct v4l2_cropcap *c = arg;
+
+	if (c->type != V4L2_BUF_TYPE_VIDEO_CAPTURE)
+	       return -EINVAL;
+
+	c->bounds.left = 0;
+	c->bounds.top = 0;
+	c->bounds.width = cam->vw.width;
+	c->bounds.height = cam->vw.height;
+	c->defrect.left = 0;
+	c->defrect.top = 0;
+	c->defrect.width = cam->vw.width;
+	c->defrect.height = cam->vw.height;
+	c->pixelaspect.numerator = 1;
+	c->pixelaspect.denominator = 1;
+
+	return 0;
+}
+
+/******************************************************************************
+ *
+ *  ioctl_queryctrl
+ *
+ *  V4L2 query possible control variables
+ *
+ *****************************************************************************/
+
+static int ioctl_queryctrl(void *arg,struct camera_data *cam)
+{
+	struct v4l2_queryctrl *c = arg;
+	int i;
+
+	for(i=0; i<NUM_CONTROLS; ++i) {
+		if(c->id == controls[i].id) {
+			memcpy(c, controls+i, sizeof(*c));
+			break;
+		}
+	}
+
+	if(i == NUM_CONTROLS)
+		return -EINVAL;
+
+	/* Some devices have additional limitations */
+	switch(c->id) {
+	case V4L2_CID_BRIGHTNESS:
+		/***
+		 * Don't let the register be set to zero - bug in VP4
+		 * flash of full brightness
+		 ***/
+		if (cam->params.pnp_id.device_type == DEVICE_STV_672)
+			c->minimum = 1;
+		break;
+	case V4L2_CID_VFLIP:
+		// VP5 Only
+		if(cam->params.pnp_id.device_type == DEVICE_STV_672)
+			c->flags |= V4L2_CTRL_FLAG_DISABLED;
+		break;
+	case CPIA2_CID_FRAMERATE:
+		if(cam->params.pnp_id.device_type == DEVICE_STV_672 &&
+		   cam->params.version.sensor_flags==CPIA2_VP_SENSOR_FLAGS_500){
+			// Maximum 15fps
+			int i;
+			for(i=0; i<c->maximum; ++i) {
+				if(framerate_controls[i].value ==
+				   CPIA2_VP_FRAMERATE_15) {
+					c->maximum = i;
+					c->default_value = i;
+				}
+			}
+		}
+		break;
+	case CPIA2_CID_FLICKER_MODE:
+		// Flicker control only valid for 672.
+		if(cam->params.pnp_id.device_type != DEVICE_STV_672)
+			c->flags |= V4L2_CTRL_FLAG_DISABLED;
+		break;
+	case CPIA2_CID_LIGHTS:
+		// Light control only valid for the QX5 Microscope.
+		if(cam->params.pnp_id.product != 0x151)
+			c->flags |= V4L2_CTRL_FLAG_DISABLED;
+		break;
+	default:
+		break;
+	}
+
+	return 0;
+}
+
+/******************************************************************************
+ *
+ *  ioctl_querymenu
+ *
+ *  V4L2 query possible control variables
+ *
+ *****************************************************************************/
+
+static int ioctl_querymenu(void *arg,struct camera_data *cam)
+{
+	struct v4l2_querymenu *m = arg;
+
+	memset(m->name, 0, sizeof(m->name));
+	m->reserved = 0;
+
+	switch(m->id) {
+	case CPIA2_CID_FLICKER_MODE:
+		if(m->index < 0 || m->index >= NUM_FLICKER_CONTROLS)
+			return -EINVAL;
+
+		strcpy(m->name, flicker_controls[m->index].name);
+		break;
+	case CPIA2_CID_FRAMERATE:
+	    {
+		int maximum = NUM_FRAMERATE_CONTROLS - 1;
+		if(cam->params.pnp_id.device_type == DEVICE_STV_672 &&
+		   cam->params.version.sensor_flags==CPIA2_VP_SENSOR_FLAGS_500){
+			// Maximum 15fps
+			int i;
+			for(i=0; i<maximum; ++i) {
+				if(framerate_controls[i].value ==
+				   CPIA2_VP_FRAMERATE_15)
+					maximum = i;
+			}
+		}
+		if(m->index < 0 || m->index > maximum)
+			return -EINVAL;
+
+		strcpy(m->name, framerate_controls[m->index].name);
+		break;
+	    }
+	case CPIA2_CID_LIGHTS:
+		if(m->index < 0 || m->index >= NUM_LIGHTS_CONTROLS)
+			return -EINVAL;
+
+		strcpy(m->name, lights_controls[m->index].name);
+		break;
+	default:
+		return -EINVAL;
+	}
+
+	return 0;
+}
+
+/******************************************************************************
+ *
+ *  ioctl_g_ctrl
+ *
+ *  V4L2 get the value of a control variable
+ *
+ *****************************************************************************/
+
+static int ioctl_g_ctrl(void *arg,struct camera_data *cam)
+{
+	struct v4l2_control *c = arg;
+
+	switch(c->id) {
+	case V4L2_CID_BRIGHTNESS:
+		cpia2_do_command(cam, CPIA2_CMD_GET_VP_BRIGHTNESS,
+				 TRANSFER_READ, 0);
+		c->value = cam->params.color_params.brightness;
+		break;
+	case V4L2_CID_CONTRAST:
+		cpia2_do_command(cam, CPIA2_CMD_GET_CONTRAST,
+				 TRANSFER_READ, 0);
+		c->value = cam->params.color_params.contrast;
+		break;
+	case V4L2_CID_SATURATION:
+		cpia2_do_command(cam, CPIA2_CMD_GET_VP_SATURATION,
+				 TRANSFER_READ, 0);
+		c->value = cam->params.color_params.saturation;
+		break;
+	case V4L2_CID_HFLIP:
+		cpia2_do_command(cam, CPIA2_CMD_GET_USER_EFFECTS,
+				 TRANSFER_READ, 0);
+		c->value = (cam->params.vp_params.user_effects &
+			    CPIA2_VP_USER_EFFECTS_MIRROR) != 0;
+		break;
+	case V4L2_CID_VFLIP:
+		cpia2_do_command(cam, CPIA2_CMD_GET_USER_EFFECTS,
+				 TRANSFER_READ, 0);
+		c->value = (cam->params.vp_params.user_effects &
+			    CPIA2_VP_USER_EFFECTS_FLIP) != 0;
+		break;
+	case CPIA2_CID_TARGET_KB:
+		c->value = cam->params.vc_params.target_kb;
+		break;
+	case CPIA2_CID_GPIO:
+		cpia2_do_command(cam, CPIA2_CMD_GET_VP_GPIO_DATA,
+				 TRANSFER_READ, 0);
+		c->value = cam->params.vp_params.gpio_data;
+		break;
+	case CPIA2_CID_FLICKER_MODE:
+	{
+		int i, mode;
+		cpia2_do_command(cam, CPIA2_CMD_GET_FLICKER_MODES,
+				 TRANSFER_READ, 0);
+		if(cam->params.flicker_control.cam_register &
+		   CPIA2_VP_FLICKER_MODES_NEVER_FLICKER) {
+			mode = NEVER_FLICKER;
+		} else {
+		    if(cam->params.flicker_control.cam_register &
+		       CPIA2_VP_FLICKER_MODES_50HZ) {
+		    	mode = FLICKER_50;
+		    } else {
+		    	mode = FLICKER_60;
+		    }
+		}
+		for(i=0; i<NUM_FLICKER_CONTROLS; i++) {
+			if(flicker_controls[i].value == mode) {
+				c->value = i;
+				break;
+			}
+		}
+		if(i == NUM_FLICKER_CONTROLS)
+			return -EINVAL;
+		break;
+	}
+	case CPIA2_CID_FRAMERATE:
+	{
+		int maximum = NUM_FRAMERATE_CONTROLS - 1;
+		int i;
+		for(i=0; i<= maximum; i++) {
+			if(cam->params.vp_params.frame_rate ==
+			   framerate_controls[i].value)
+				break;
+		}
+		if(i > maximum)
+			return -EINVAL;
+		c->value = i;
+		break;
+	}
+	case CPIA2_CID_USB_ALT:
+		c->value = cam->params.camera_state.stream_mode;
+		break;
+	case CPIA2_CID_LIGHTS:
+	{
+		int i;
+		cpia2_do_command(cam, CPIA2_CMD_GET_VP_GPIO_DATA,
+				 TRANSFER_READ, 0);
+		for(i=0; i<NUM_LIGHTS_CONTROLS; i++) {
+			if((cam->params.vp_params.gpio_data&GPIO_LIGHTS_MASK) ==
+			   lights_controls[i].value) {
+				break;
+			}
+		}
+		if(i == NUM_LIGHTS_CONTROLS)
+			return -EINVAL;
+		c->value = i;
+		break;
+	}
+	case CPIA2_CID_RESET_CAMERA:
+		return -EINVAL;
+	default:
+		return -EINVAL;
+	}
+
+	DBG("Get control id:%d, value:%d\n", c->id, c->value);
+
+	return 0;
+}
+
+/******************************************************************************
+ *
+ *  ioctl_s_ctrl
+ *
+ *  V4L2 set the value of a control variable
+ *
+ *****************************************************************************/
+
+static int ioctl_s_ctrl(void *arg,struct camera_data *cam)
+{
+	struct v4l2_control *c = arg;
+	int i;
+	int retval = 0;
+
+	DBG("Set control id:%d, value:%d\n", c->id, c->value);
+
+	/* Check that the value is in range */
+	for(i=0; i<NUM_CONTROLS; i++) {
+		if(c->id == controls[i].id) {
+			if(c->value < controls[i].minimum ||
+			   c->value > controls[i].maximum) {
+				return -EINVAL;
+			}
+			break;
+		}
+	}
+	if(i == NUM_CONTROLS)
+		return -EINVAL;
+
+	switch(c->id) {
+	case V4L2_CID_BRIGHTNESS:
+		cpia2_set_brightness(cam, c->value);
+		break;
+	case V4L2_CID_CONTRAST:
+		cpia2_set_contrast(cam, c->value);
+		break;
+	case V4L2_CID_SATURATION:
+		cpia2_set_saturation(cam, c->value);
+		break;
+	case V4L2_CID_HFLIP:
+		cpia2_set_property_mirror(cam, c->value);
+		break;
+	case V4L2_CID_VFLIP:
+		cpia2_set_property_flip(cam, c->value);
+		break;
+	case CPIA2_CID_TARGET_KB:
+		retval = cpia2_set_target_kb(cam, c->value);
+		break;
+	case CPIA2_CID_GPIO:
+		retval = cpia2_set_gpio(cam, c->value);
+		break;
+	case CPIA2_CID_FLICKER_MODE:
+		retval = cpia2_set_flicker_mode(cam,
+					      flicker_controls[c->value].value);
+		break;
+	case CPIA2_CID_FRAMERATE:
+		retval = cpia2_set_fps(cam, framerate_controls[c->value].value);
+		break;
+	case CPIA2_CID_USB_ALT:
+		retval = cpia2_usb_change_streaming_alternate(cam, c->value);
+		break;
+	case CPIA2_CID_LIGHTS:
+		retval = cpia2_set_gpio(cam, lights_controls[c->value].value);
+		break;
+	case CPIA2_CID_RESET_CAMERA:
+		cpia2_usb_stream_pause(cam);
+		cpia2_reset_camera(cam);
+		cpia2_usb_stream_resume(cam);
+		break;
+	default:
+		retval = -EINVAL;
+	}
+
+	return retval;
+}
+
+/******************************************************************************
+ *
+ *  ioctl_g_jpegcomp
+ *
+ *  V4L2 get the JPEG compression parameters
+ *
+ *****************************************************************************/
+
+static int ioctl_g_jpegcomp(void *arg,struct camera_data *cam)
+{
+	struct v4l2_jpegcompression *parms = arg;
+
+	memset(parms, 0, sizeof(*parms));
+
+	parms->quality = 80; // TODO: Can this be made meaningful?
+
+	parms->jpeg_markers = V4L2_JPEG_MARKER_DQT | V4L2_JPEG_MARKER_DRI;
+	if(!cam->params.compression.inhibit_htables) {
+		parms->jpeg_markers |= V4L2_JPEG_MARKER_DHT;
+	}
+
+	parms->APPn = cam->APPn;
+	parms->APP_len = cam->APP_len;
+	if(cam->APP_len > 0) {
+		memcpy(parms->APP_data, cam->APP_data, cam->APP_len);
+		parms->jpeg_markers |= V4L2_JPEG_MARKER_APP;
+	}
+
+	parms->COM_len = cam->COM_len;
+	if(cam->COM_len > 0) {
+		memcpy(parms->COM_data, cam->COM_data, cam->COM_len);
+		parms->jpeg_markers |= JPEG_MARKER_COM;
+	}
+
+	DBG("G_JPEGCOMP APP_len:%d COM_len:%d\n",
+	    parms->APP_len, parms->COM_len);
+
+	return 0;
+}
+
+/******************************************************************************
+ *
+ *  ioctl_s_jpegcomp
+ *
+ *  V4L2 set the JPEG compression parameters
+ *  NOTE: quality and some jpeg_markers are ignored.
+ *
+ *****************************************************************************/
+
+static int ioctl_s_jpegcomp(void *arg,struct camera_data *cam)
+{
+	struct v4l2_jpegcompression *parms = arg;
+
+	DBG("S_JPEGCOMP APP_len:%d COM_len:%d\n",
+	    parms->APP_len, parms->COM_len);
+
+	cam->params.compression.inhibit_htables =
+		!(parms->jpeg_markers & V4L2_JPEG_MARKER_DHT);
+
+	if(parms->APP_len != 0) {
+		if(parms->APP_len > 0 &&
+		   parms->APP_len <= sizeof(cam->APP_data) &&
+		   parms->APPn >= 0 && parms->APPn <= 15) {
+			cam->APPn = parms->APPn;
+			cam->APP_len = parms->APP_len;
+			memcpy(cam->APP_data, parms->APP_data, parms->APP_len);
+		} else {
+			LOG("Bad APPn Params n=%d len=%d\n",
+			    parms->APPn, parms->APP_len);
+			return -EINVAL;
+		}
+	} else {
+		cam->APP_len = 0;
+	}
+
+	if(parms->COM_len != 0) {
+		if(parms->COM_len > 0 &&
+		   parms->COM_len <= sizeof(cam->COM_data)) {
+			cam->COM_len = parms->COM_len;
+			memcpy(cam->COM_data, parms->COM_data, parms->COM_len);
+		} else {
+			LOG("Bad COM_len=%d\n", parms->COM_len);
+			return -EINVAL;
+		}
+	}
+
+	return 0;
+}
+
+/******************************************************************************
+ *
+ *  ioctl_reqbufs
+ *
+ *  V4L2 Initiate memory mapping.
+ *  NOTE: The user's request is ignored. For now the buffers are fixed.
+ *
+ *****************************************************************************/
+
+static int ioctl_reqbufs(void *arg,struct camera_data *cam)
+{
+	struct v4l2_requestbuffers *req = arg;
+
+	if(req->type != V4L2_BUF_TYPE_VIDEO_CAPTURE ||
+	   req->memory != V4L2_MEMORY_MMAP)
+		return -EINVAL;
+
+	DBG("REQBUFS requested:%d returning:%d\n", req->count, cam->num_frames);
+	req->count = cam->num_frames;
+	memset(&req->reserved, 0, sizeof(req->reserved));
+
+	return 0;
+}
+
+/******************************************************************************
+ *
+ *  ioctl_querybuf
+ *
+ *  V4L2 Query memory buffer status.
+ *
+ *****************************************************************************/
+
+static int ioctl_querybuf(void *arg,struct camera_data *cam)
+{
+	struct v4l2_buffer *buf = arg;
+
+	if(buf->type != V4L2_BUF_TYPE_VIDEO_CAPTURE ||
+	   buf->index > cam->num_frames)
+		return -EINVAL;
+
+	buf->m.offset = cam->buffers[buf->index].data - cam->frame_buffer;
+	buf->length = cam->frame_size;
+
+	buf->memory = V4L2_MEMORY_MMAP;
+
+	if(cam->mmapped)
+		buf->flags = V4L2_BUF_FLAG_MAPPED;
+	else
+		buf->flags = 0;
+
+	switch (cam->buffers[buf->index].status) {
+	case FRAME_EMPTY:
+	case FRAME_ERROR:
+	case FRAME_READING:
+		buf->bytesused = 0;
+		buf->flags = V4L2_BUF_FLAG_QUEUED;
+		break;
+	case FRAME_READY:
+		buf->bytesused = cam->buffers[buf->index].length;
+		buf->timestamp = cam->buffers[buf->index].timestamp;
+		buf->sequence = cam->buffers[buf->index].seq;
+		buf->flags = V4L2_BUF_FLAG_DONE;
+		break;
+	}
+
+	DBG("QUERYBUF index:%d offset:%d flags:%d seq:%d bytesused:%d\n",
+	     buf->index, buf->m.offset, buf->flags, buf->sequence,
+	     buf->bytesused);
+
+	return 0;
+}
+
+/******************************************************************************
+ *
+ *  ioctl_qbuf
+ *
+ *  V4L2 User is freeing buffer
+ *
+ *****************************************************************************/
+
+static int ioctl_qbuf(void *arg,struct camera_data *cam)
+{
+	struct v4l2_buffer *buf = arg;
+
+	if(buf->type != V4L2_BUF_TYPE_VIDEO_CAPTURE ||
+	   buf->memory != V4L2_MEMORY_MMAP ||
+	   buf->index > cam->num_frames)
+		return -EINVAL;
+
+	DBG("QBUF #%d\n", buf->index);
+
+	if(cam->buffers[buf->index].status == FRAME_READY)
+		cam->buffers[buf->index].status = FRAME_EMPTY;
+
+	return 0;
+}
+
+/******************************************************************************
+ *
+ *  find_earliest_filled_buffer
+ *
+ *  Helper for ioctl_dqbuf. Find the next ready buffer.
+ *
+ *****************************************************************************/
+
+static int find_earliest_filled_buffer(struct camera_data *cam)
+{
+	int i;
+	int found = -1;
+	for (i=0; i<cam->num_frames; i++) {
+		if(cam->buffers[i].status == FRAME_READY) {
+			if(found < 0) {
+				found = i;
+			} else {
+				/* find which buffer is earlier */
+				struct timeval *tv1, *tv2;
+				tv1 = &cam->buffers[i].timestamp;
+				tv2 = &cam->buffers[found].timestamp;
+				if(tv1->tv_sec < tv2->tv_sec ||
+				   (tv1->tv_sec == tv2->tv_sec &&
+				    tv1->tv_usec < tv2->tv_usec))
+					found = i;
+			}
+		}
+	}
+	return found;
+}
+
+/******************************************************************************
+ *
+ *  ioctl_dqbuf
+ *
+ *  V4L2 User is asking for a filled buffer.
+ *
+ *****************************************************************************/
+
+static int ioctl_dqbuf(void *arg,struct camera_data *cam, struct file *file)
+{
+	struct v4l2_buffer *buf = arg;
+	int frame;
+
+	if(buf->type != V4L2_BUF_TYPE_VIDEO_CAPTURE ||
+	   buf->memory != V4L2_MEMORY_MMAP)
+		return -EINVAL;
+
+	frame = find_earliest_filled_buffer(cam);
+
+	if(frame < 0 && file->f_flags&O_NONBLOCK)
+		return -EAGAIN;
+
+	if(frame < 0) {
+		/* Wait for a frame to become available */
+		struct framebuf *cb=cam->curbuff;
+		up(&cam->busy_lock);
+		wait_event_interruptible(cam->wq_stream,
+					 !cam->present ||
+					 (cb=cam->curbuff)->status == FRAME_READY);
+		down(&cam->busy_lock);
+		if (signal_pending(current))
+			return -ERESTARTSYS;
+		if(!cam->present)
+			return -ENOTTY;
+		frame = cb->num;
+	}
+
+
+	buf->index = frame;
+	buf->bytesused = cam->buffers[buf->index].length;
+	buf->flags = V4L2_BUF_FLAG_MAPPED | V4L2_BUF_FLAG_DONE;
+	buf->field = V4L2_FIELD_NONE;
+	buf->timestamp = cam->buffers[buf->index].timestamp;
+	buf->sequence = cam->buffers[buf->index].seq;
+	buf->m.offset = cam->buffers[buf->index].data - cam->frame_buffer;
+	buf->length = cam->frame_size;
+	buf->input = 0;
+	buf->reserved = 0;
+	memset(&buf->timecode, 0, sizeof(buf->timecode));
+
+	DBG("DQBUF #%d status:%d seq:%d length:%d\n", buf->index,
+	    cam->buffers[buf->index].status, buf->sequence, buf->bytesused);
+
+	return 0;
+}
+
+/******************************************************************************
+ *
+ *  cpia2_ioctl
+ *
+ *****************************************************************************/
+static int cpia2_do_ioctl(struct inode *inode, struct file *file,
+			  unsigned int ioctl_nr, void *arg)
+{
+	struct video_device *dev = video_devdata(file);
+	struct camera_data *cam = video_get_drvdata(dev);
+	int retval = 0;
+
+	if (!cam)
+		return -ENOTTY;
+
+	/* make this _really_ smp-safe */
+	if (down_interruptible(&cam->busy_lock))
+		return -ERESTARTSYS;
+
+	if (!cam->present) {
+		up(&cam->busy_lock);
+		return -ENODEV;
+	}
+
+	/* Priority check */
+	switch (ioctl_nr) {
+	case VIDIOCSWIN:
+	case VIDIOCMCAPTURE:
+	case VIDIOC_S_FMT:
+	{
+		struct cpia2_fh *fh = file->private_data;
+		retval = v4l2_prio_check(&cam->prio, &fh->prio);
+		if(retval) {
+			up(&cam->busy_lock);
+			return retval;
+		}
+		break;
+	}
+	case VIDIOCGMBUF:
+	case VIDIOCSYNC:
+	{
+		struct cpia2_fh *fh = file->private_data;
+		if(fh->prio != V4L2_PRIORITY_RECORD) {
+			up(&cam->busy_lock);
+			return -EBUSY;
+		}
+		break;
+	}
+	default:
+		break;
+	}
+
+	switch (ioctl_nr) {
+	case VIDIOCGCAP:	/* query capabilites */
+		retval = ioctl_cap_query(arg, cam);
+		break;
+
+	case VIDIOCGCHAN:	/* get video source - we are a camera, nothing else */
+		retval = ioctl_get_channel(arg);
+		break;
+	case VIDIOCSCHAN:	/* set video source - we are a camera, nothing else */
+		retval = ioctl_set_channel(arg);
+		break;
+	case VIDIOCGPICT:	/* image properties */
+		memcpy(arg, &cam->vp, sizeof(struct video_picture));
+		break;
+	case VIDIOCSPICT:
+		retval = ioctl_set_image_prop(arg, cam);
+		break;
+	case VIDIOCGWIN:	/* get/set capture window */
+		memcpy(arg, &cam->vw, sizeof(struct video_window));
+		break;
+	case VIDIOCSWIN:
+		retval = ioctl_set_window_size(arg, cam, file->private_data);
+		break;
+	case VIDIOCGMBUF:	/* mmap interface */
+		retval = ioctl_get_mbuf(arg, cam);
+		break;
+	case VIDIOCMCAPTURE:
+		retval = ioctl_mcapture(arg, cam, file->private_data);
+		break;
+	case VIDIOCSYNC:
+		retval = ioctl_sync(arg, cam);
+		break;
+		/* pointless to implement overlay with this camera */
+	case VIDIOCCAPTURE:
+	case VIDIOCGFBUF:
+	case VIDIOCSFBUF:
+	case VIDIOCKEY:
+		retval = -EINVAL;
+		break;
+
+		/* tuner interface - we have none */
+	case VIDIOCGTUNER:
+	case VIDIOCSTUNER:
+	case VIDIOCGFREQ:
+	case VIDIOCSFREQ:
+		retval = -EINVAL;
+		break;
+
+		/* audio interface - we have none */
+	case VIDIOCGAUDIO:
+	case VIDIOCSAUDIO:
+		retval = -EINVAL;
+		break;
+
+	/* CPIA2 extension to Video4Linux API */
+	case CPIA2_IOC_SET_GPIO:
+		retval = ioctl_set_gpio(arg, cam);
+		break;
+	case VIDIOC_QUERYCAP:
+		retval = ioctl_querycap(arg,cam);
+		break;
+
+	case VIDIOC_ENUMINPUT:
+	case VIDIOC_G_INPUT:
+	case VIDIOC_S_INPUT:
+		retval = ioctl_input(ioctl_nr, arg,cam);
+		break;
+
+	case VIDIOC_ENUM_FMT:
+		retval = ioctl_enum_fmt(arg,cam);
+		break;
+	case VIDIOC_TRY_FMT:
+		retval = ioctl_try_fmt(arg,cam);
+		break;
+	case VIDIOC_G_FMT:
+		retval = ioctl_get_fmt(arg,cam);
+		break;
+	case VIDIOC_S_FMT:
+		retval = ioctl_set_fmt(arg,cam,file->private_data);
+		break;
+
+	case VIDIOC_CROPCAP:
+		retval = ioctl_cropcap(arg,cam);
+		break;
+	case VIDIOC_G_CROP:
+	case VIDIOC_S_CROP:
+		// TODO: I think cropping can be implemented - SJB
+		retval = -EINVAL;
+		break;
+
+	case VIDIOC_QUERYCTRL:
+		retval = ioctl_queryctrl(arg,cam);
+		break;
+	case VIDIOC_QUERYMENU:
+		retval = ioctl_querymenu(arg,cam);
+		break;
+	case VIDIOC_G_CTRL:
+		retval = ioctl_g_ctrl(arg,cam);
+		break;
+	case VIDIOC_S_CTRL:
+		retval = ioctl_s_ctrl(arg,cam);
+		break;
+
+	case VIDIOC_G_JPEGCOMP:
+		retval = ioctl_g_jpegcomp(arg,cam);
+		break;
+	case VIDIOC_S_JPEGCOMP:
+		retval = ioctl_s_jpegcomp(arg,cam);
+		break;
+
+	case VIDIOC_G_PRIORITY:
+	{
+		struct cpia2_fh *fh = file->private_data;
+		*(enum v4l2_priority*)arg = fh->prio;
+		break;
+	}
+	case VIDIOC_S_PRIORITY:
+	{
+		struct cpia2_fh *fh = file->private_data;
+		enum v4l2_priority prio;
+		prio = *(enum v4l2_priority*)arg;
+		if(cam->streaming &&
+		   prio != fh->prio &&
+		   fh->prio == V4L2_PRIORITY_RECORD) {
+			/* Can't drop record priority while streaming */
+			retval = -EBUSY;
+		} else if(prio == V4L2_PRIORITY_RECORD &&
+		   prio != fh->prio &&
+		   v4l2_prio_max(&cam->prio) == V4L2_PRIORITY_RECORD) {
+			/* Only one program can record at a time */
+			retval = -EBUSY;
+		} else {
+			retval = v4l2_prio_change(&cam->prio, &fh->prio, prio);
+		}
+		break;
+	}
+
+	case VIDIOC_REQBUFS:
+		retval = ioctl_reqbufs(arg,cam);
+		break;
+	case VIDIOC_QUERYBUF:
+		retval = ioctl_querybuf(arg,cam);
+		break;
+	case VIDIOC_QBUF:
+		retval = ioctl_qbuf(arg,cam);
+		break;
+	case VIDIOC_DQBUF:
+		retval = ioctl_dqbuf(arg,cam,file);
+		break;
+	case VIDIOC_STREAMON:
+	{
+		int type;
+		DBG("VIDIOC_STREAMON, streaming=%d\n", cam->streaming);
+		type = *(int*)arg;
+		if(!cam->mmapped || type != V4L2_BUF_TYPE_VIDEO_CAPTURE)
+			retval = -EINVAL;
+
+		if(!cam->streaming) {
+			retval = cpia2_usb_stream_start(cam,
+					  cam->params.camera_state.stream_mode);
+		} else {
+			retval = -EINVAL;
+		}
+
+		break;
+	}
+	case VIDIOC_STREAMOFF:
+	{
+		int type;
+		DBG("VIDIOC_STREAMOFF, streaming=%d\n", cam->streaming);
+		type = *(int*)arg;
+		if(!cam->mmapped || type != V4L2_BUF_TYPE_VIDEO_CAPTURE)
+			retval = -EINVAL;
+
+		if(cam->streaming) {
+			retval = cpia2_usb_stream_stop(cam);
+		} else {
+			retval = -EINVAL;
+		}
+
+		break;
+	}
+
+	case VIDIOC_ENUMOUTPUT:
+	case VIDIOC_G_OUTPUT:
+	case VIDIOC_S_OUTPUT:
+	case VIDIOC_G_MODULATOR:
+	case VIDIOC_S_MODULATOR:
+
+	case VIDIOC_ENUMAUDIO:
+	case VIDIOC_G_AUDIO:
+	case VIDIOC_S_AUDIO:
+
+	case VIDIOC_ENUMAUDOUT:
+	case VIDIOC_G_AUDOUT:
+	case VIDIOC_S_AUDOUT:
+
+	case VIDIOC_ENUMSTD:
+	case VIDIOC_QUERYSTD:
+	case VIDIOC_G_STD:
+	case VIDIOC_S_STD:
+
+	case VIDIOC_G_TUNER:
+	case VIDIOC_S_TUNER:
+	case VIDIOC_G_FREQUENCY:
+	case VIDIOC_S_FREQUENCY:
+
+	case VIDIOC_OVERLAY:
+	case VIDIOC_G_FBUF:
+	case VIDIOC_S_FBUF:
+
+	case VIDIOC_G_PARM:
+	case VIDIOC_S_PARM:
+		retval = -EINVAL;
+		break;
+	default:
+		retval = -ENOIOCTLCMD;
+		break;
+	}
+
+	up(&cam->busy_lock);
+	return retval;
+}
+
+static int cpia2_ioctl(struct inode *inode, struct file *file,
+		       unsigned int ioctl_nr, unsigned long iarg)
+{
+	return video_usercopy(inode, file, ioctl_nr, iarg, cpia2_do_ioctl);
+}
+
+/******************************************************************************
+ *
+ *  cpia2_mmap
+ *
+ *****************************************************************************/
+static int cpia2_mmap(struct file *file, struct vm_area_struct *area)
+{
+	int retval;
+	struct video_device *dev = video_devdata(file);
+	struct camera_data *cam = video_get_drvdata(dev);
+
+	/* Priority check */
+	struct cpia2_fh *fh = file->private_data;
+	if(fh->prio != V4L2_PRIORITY_RECORD) {
+		return -EBUSY;
+	}
+
+	retval = cpia2_remap_buffer(cam, area);
+
+	if(!retval)
+		fh->mmapped = 1;
+	return retval;
+}
+
+/******************************************************************************
+ *
+ *  reset_camera_struct_v4l
+ *
+ *  Sets all values to the defaults
+ *****************************************************************************/
+static void reset_camera_struct_v4l(struct camera_data *cam)
+{
+	/***
+	 * Fill in the v4l structures.  video_cap is filled in inside the VIDIOCCAP
+	 * Ioctl.  Here, just do the window and picture stucts.
+	 ***/
+	cam->vp.palette = (u16) VIDEO_PALETTE_RGB24;	/* Is this right? */
+	cam->vp.brightness = (u16) cam->params.color_params.brightness * 256;
+	cam->vp.colour = (u16) cam->params.color_params.saturation * 256;
+	cam->vp.contrast = (u16) cam->params.color_params.contrast * 256;
+
+	cam->vw.x = 0;
+	cam->vw.y = 0;
+	cam->vw.width = cam->params.roi.width;
+	cam->vw.height = cam->params.roi.height;
+	cam->vw.flags = 0;
+	cam->vw.clipcount = 0;
+
+	cam->frame_size = buffer_size;
+	cam->num_frames = num_buffers;
+
+	/* FlickerModes */
+	cam->params.flicker_control.flicker_mode_req = flicker_mode;
+	cam->params.flicker_control.mains_frequency = flicker_freq;
+
+	/* streamMode */
+	cam->params.camera_state.stream_mode = alternate;
+
+	cam->pixelformat = V4L2_PIX_FMT_JPEG;
+	v4l2_prio_init(&cam->prio);
+	return;
+}
+
+/***
+ * The v4l video device structure initialized for this device
+ ***/
+static struct file_operations fops_template = {
+	.owner=      THIS_MODULE,
+	.open=       cpia2_open,
+	.release=    cpia2_close,
+	.read=       cpia2_v4l_read,
+	.poll=       cpia2_v4l_poll,
+	.ioctl=      cpia2_ioctl,
+	.llseek=     no_llseek,
+	.mmap=       cpia2_mmap,
+};
+
+static struct video_device cpia2_template = {
+	/* I could not find any place for the old .initialize initializer?? */
+	.owner=		THIS_MODULE,
+	.name=		"CPiA2 Camera",
+	.type=		VID_TYPE_CAPTURE,
+	.type2 = 	V4L2_CAP_VIDEO_CAPTURE |
+			V4L2_CAP_STREAMING,
+	.hardware=	VID_HARDWARE_CPIA2,
+	.minor=		-1,
+	.fops=		&fops_template,
+	.release=	video_device_release,
+};
+
+/******************************************************************************
+ *
+ *  cpia2_register_camera
+ *
+ *****************************************************************************/
+int cpia2_register_camera(struct camera_data *cam)
+{
+	cam->vdev = video_device_alloc();
+	if(!cam->vdev)
+		return -ENOMEM;
+
+	memcpy(cam->vdev, &cpia2_template, sizeof(cpia2_template));
+	video_set_drvdata(cam->vdev, cam);
+
+	reset_camera_struct_v4l(cam);
+
+	/* register v4l device */
+	if (video_register_device
+	    (cam->vdev, VFL_TYPE_GRABBER, video_nr) == -1) {
+		ERR("video_register_device failed\n");
+		video_device_release(cam->vdev);
+		return -ENODEV;
+	}
+
+	return 0;
+}
+
+/******************************************************************************
+ *
+ *  cpia2_unregister_camera
+ *
+ *****************************************************************************/
+void cpia2_unregister_camera(struct camera_data *cam)
+{
+	if (!cam->open_count) {
+		video_unregister_device(cam->vdev);
+	} else {
+		LOG("/dev/video%d removed while open, "
+		    "deferring video_unregister_device\n",
+		    cam->vdev->minor);
+	}
+}
+
+/******************************************************************************
+ *
+ *  check_parameters
+ *
+ *  Make sure that all user-supplied parameters are sensible
+ *****************************************************************************/
+static void __init check_parameters(void)
+{
+	if(buffer_size < PAGE_SIZE) {
+		buffer_size = PAGE_SIZE;
+		LOG("buffer_size too small, setting to %d\n", buffer_size);
+	} else if(buffer_size > 1024*1024) {
+		/* arbitrary upper limiit */
+		buffer_size = 1024*1024;
+		LOG("buffer_size ridiculously large, setting to %d\n",
+		    buffer_size);
+	} else {
+		buffer_size += PAGE_SIZE-1;
+		buffer_size &= ~(PAGE_SIZE-1);
+	}
+
+	if(num_buffers < 1) {
+		num_buffers = 1;
+		LOG("num_buffers too small, setting to %d\n", num_buffers);
+	} else if(num_buffers > VIDEO_MAX_FRAME) {
+		num_buffers = VIDEO_MAX_FRAME;
+		LOG("num_buffers too large, setting to %d\n", num_buffers);
+	}
+
+	if(alternate < USBIF_ISO_1 || alternate > USBIF_ISO_6) {
+		alternate = DEFAULT_ALT;
+		LOG("alternate specified is invalid, using %d\n", alternate);
+	}
+
+	if (flicker_mode != NEVER_FLICKER && flicker_mode != ANTI_FLICKER_ON) {
+		flicker_mode = NEVER_FLICKER;
+		LOG("Flicker mode specified is invalid, using %d\n",
+		    flicker_mode);
+	}
+
+	if (flicker_freq != FLICKER_50 && flicker_freq != FLICKER_60) {
+		flicker_freq = FLICKER_60;
+		LOG("Flicker mode specified is invalid, using %d\n",
+		    flicker_freq);
+	}
+
+	if(video_nr < -1 || video_nr > 64) {
+		video_nr = -1;
+		LOG("invalid video_nr specified, must be -1 to 64\n");
+	}
+
+	DBG("Using %d buffers, each %d bytes, alternate=%d\n",
+	    num_buffers, buffer_size, alternate);
+}
+
+/************   Module Stuff ***************/
+
+
+/******************************************************************************
+ *
+ * cpia2_init/module_init
+ *
+ *****************************************************************************/
+int __init cpia2_init(void)
+{
+	LOG("%s v%d.%d.%d\n",
+	    ABOUT, CPIA2_MAJ_VER, CPIA2_MIN_VER, CPIA2_PATCH_VER);
+	check_parameters();
+	cpia2_usb_init();
+	return 0;
+}
+
+
+/******************************************************************************
+ *
+ * cpia2_exit/module_exit
+ *
+ *****************************************************************************/
+void __exit cpia2_exit(void)
+{
+	cpia2_usb_cleanup();
+	schedule_timeout(2 * HZ);
+}
+
+
+int __init cpia2_setup(char *str)
+{
+	while(str) {
+		if(!strncmp(str, "buffer_size:", 12)) {
+			buffer_size = simple_strtoul(str + 13, &str, 10);
+		} else if(!strncmp(str, "num_buffers:", 12)) {
+			num_buffers = simple_strtoul(str + 13, &str, 10);
+		} else if(!strncmp(str, "alternate:", 10)) {
+			alternate = simple_strtoul(str + 11, &str, 10);
+		} else if(!strncmp(str, "video_nr:", 9)) {
+			video_nr = simple_strtoul(str + 10, &str, 10);
+		} else if(!strncmp(str, "flicker_freq:",13)) {
+		   flicker_freq = simple_strtoul(str + 14, &str, 10);
+		} else if(!strncmp(str, "flicker_mode:",13)) {
+		   flicker_mode = simple_strtoul(str + 14, &str, 10);
+		} else {
+			++str;
+		}
+	}
+	return 1;
+}
+
+__setup("cpia2=", cpia2_setup);
+
+module_init(cpia2_init);
+module_exit(cpia2_exit);
+
diff --git a/drivers/media/video/cpia2/cpia2dev.h b/drivers/media/video/cpia2/cpia2dev.h
new file mode 100644
index 00000000000000..d58097ce0d5e79
--- /dev/null
+++ b/drivers/media/video/cpia2/cpia2dev.h
@@ -0,0 +1,50 @@
+/****************************************************************************
+ *
+ *  Filename: cpia2dev.h
+ *
+ *  Copyright 2001, STMicrolectronics, Inc.
+ *
+ *  Contact:  steve.miller@st.com
+ *
+ *  Description:
+ *     This file provides definitions for applications wanting to use the
+ *     cpia2 driver beyond the generic v4l capabilities.
+ *
+ *  This program is free software; you can redistribute it and/or modify
+ *  it under the terms of the GNU General Public License as published by
+ *  the Free Software Foundation; either version 2 of the License, or
+ *  (at your option) any later version.
+ *
+ *  This program is distributed in the hope that it will be useful,
+ *  but WITHOUT ANY WARRANTY; without even the implied warranty of
+ *  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ *  GNU General Public License for more details.
+ *
+ *  You should have received a copy of the GNU General Public License
+ *  along with this program; if not, write to the Free Software
+ *  Foundation, Inc., 675 Mass Ave, Cambridge, MA 02139, USA.
+ *
+ ****************************************************************************/
+
+#ifndef CPIA2_DEV_HEADER
+#define CPIA2_DEV_HEADER
+
+#include <linux/videodev.h>
+
+/***
+ * The following defines are ioctl numbers based on video4linux private ioctls,
+ * which can range from 192 (BASE_VIDIOCPRIVATE) to 255. All of these take int
+ * args
+ */
+#define CPIA2_IOC_SET_GPIO         _IOW('v', BASE_VIDIOCPRIVATE + 17, __u32)
+
+/* V4L2 driver specific controls */
+#define CPIA2_CID_TARGET_KB     (V4L2_CID_PRIVATE_BASE+0)
+#define CPIA2_CID_GPIO          (V4L2_CID_PRIVATE_BASE+1)
+#define CPIA2_CID_FLICKER_MODE  (V4L2_CID_PRIVATE_BASE+2)
+#define CPIA2_CID_FRAMERATE     (V4L2_CID_PRIVATE_BASE+3)
+#define CPIA2_CID_USB_ALT       (V4L2_CID_PRIVATE_BASE+4)
+#define CPIA2_CID_LIGHTS        (V4L2_CID_PRIVATE_BASE+5)
+#define CPIA2_CID_RESET_CAMERA  (V4L2_CID_PRIVATE_BASE+6)
+
+#endif
diff --git a/drivers/media/video/cpia2/cpia2patch.h b/drivers/media/video/cpia2/cpia2patch.h
new file mode 100644
index 00000000000000..7f085fbe76fb28
--- /dev/null
+++ b/drivers/media/video/cpia2/cpia2patch.h
@@ -0,0 +1,233 @@
+/****************************************************************************
+ *
+ *  Filename: cpia2patch.h
+ *
+ *  Copyright 2001, STMicrolectronics, Inc.
+ *
+ *  Contact:  steve.miller@st.com
+ *
+ *  Description:
+ *     This file contains patch data for the CPiA2 (stv0672) VP4.
+ *
+ *  This program is free software; you can redistribute it and/or modify
+ *  it under the terms of the GNU General Public License as published by
+ *  the Free Software Foundation; either version 2 of the License, or
+ *  (at your option) any later version.
+ *
+ *  This program is distributed in the hope that it will be useful,
+ *  but WITHOUT ANY WARRANTY; without even the implied warranty of
+ *  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ *  GNU General Public License for more details.
+ *
+ *  You should have received a copy of the GNU General Public License
+ *  along with this program; if not, write to the Free Software
+ *  Foundation, Inc., 675 Mass Ave, Cambridge, MA 02139, USA.
+ *
+ ****************************************************************************/
+
+#ifndef CPIA2_PATCH_HEADER
+#define CPIA2_PATCH_HEADER
+
+typedef struct {
+	unsigned char reg;
+	unsigned char count;
+	const unsigned char *data;
+} cpia2_patch;
+
+static const unsigned char start_address_hi[1] = {
+	0x01
+};
+
+static const unsigned char start_address_lo[1] = {
+	0xBC
+};
+
+static const unsigned char patch_block0[64] = {
+	0xE3, 0x02, 0xE3, 0x03, 0xE3, 0x04, 0xE3, 0x05,
+	0xE3, 0x06, 0xE3, 0x07, 0x93, 0x44, 0x56, 0xD4,
+	0x93, 0x4E, 0x56, 0x51, 0x93, 0x4E, 0x51, 0xD6,
+	0x93, 0x4E, 0x4F, 0x54, 0x93, 0x4E, 0x92, 0x4F,
+	0x92, 0xA4, 0x93, 0x05, 0x92, 0xF4, 0x93, 0x1B,
+	0x92, 0x92, 0x91, 0xE6, 0x92, 0x36, 0x92, 0x74,
+	0x92, 0x4A, 0x92, 0x8C, 0x92, 0x8E, 0xC8, 0xD0,
+	0x0B, 0x42, 0x02, 0xA0, 0xCA, 0x92, 0x09, 0x02
+};
+
+static const unsigned char patch_block1[64] = {
+	0xC9, 0x10, 0x0A, 0x0A, 0x0A, 0x81, 0xE3, 0xB8,
+	0xE3, 0xB0, 0xE3, 0xA8, 0xE3, 0xA0, 0xE3, 0x98,
+	0xE3, 0x90, 0xE1, 0x00, 0xCF, 0xD7, 0x0A, 0x12,
+	0xCC, 0x95, 0x08, 0xB2, 0x0A, 0x18, 0xE1, 0x00,
+	0x01, 0xEE, 0x0C, 0x08, 0x4A, 0x12, 0xC8, 0x18,
+	0xF0, 0x9A, 0xC0, 0x22, 0xF3, 0x1C, 0x4A, 0x13,
+	0xF3, 0x14, 0xC8, 0xA0, 0xF2, 0x14, 0xF2, 0x1C,
+	0xEB, 0x13, 0xD3, 0xA2, 0x63, 0x16, 0x48, 0x9E
+};
+
+static const unsigned char patch_block2[64] = {
+	0xF0, 0x18, 0xA4, 0x03, 0xF3, 0x93, 0xC0, 0x58,
+	0xF7, 0x13, 0x51, 0x9C, 0xE9, 0x20, 0xCF, 0xEF,
+	0x63, 0xF9, 0x92, 0x2E, 0xD3, 0x5F, 0x63, 0xFA,
+	0x92, 0x2E, 0xD3, 0x67, 0x63, 0xFB, 0x92, 0x2E,
+	0xD3, 0x6F, 0xE9, 0x1A, 0x63, 0x16, 0x48, 0xA7,
+	0xF0, 0x20, 0xA4, 0x06, 0xF3, 0x94, 0xC0, 0x27,
+	0xF7, 0x14, 0xF5, 0x13, 0x51, 0x9D, 0xF6, 0x13,
+	0x63, 0x18, 0xC4, 0x20, 0xCB, 0xEF, 0x63, 0xFC
+};
+
+static const unsigned char patch_block3[64] = {
+	0x92, 0x2E, 0xD3, 0x77, 0x63, 0xFD, 0x92, 0x2E,
+	0xD3, 0x7F, 0x63, 0xFE, 0x92, 0x2E, 0xD3, 0x87,
+	0x63, 0xFF, 0x92, 0x2E, 0xD3, 0x8F, 0x64, 0x38,
+	0x92, 0x2E, 0xD3, 0x97, 0x64, 0x39, 0x92, 0x2E,
+	0xD3, 0x9F, 0xE1, 0x00, 0xF5, 0x3A, 0xF4, 0x3B,
+	0xF7, 0xBF, 0xF2, 0xBC, 0xF2, 0x3D, 0xE1, 0x00,
+	0x80, 0x87, 0x90, 0x80, 0x51, 0xD5, 0x02, 0x22,
+	0x02, 0x32, 0x4B, 0xD3, 0xF7, 0x11, 0x0B, 0xDA
+};
+
+static const unsigned char patch_block4[64] = {
+	0xE1, 0x00, 0x0E, 0x02, 0x02, 0x40, 0x0D, 0xB5,
+	0xE3, 0x02, 0x48, 0x55, 0xE5, 0x12, 0xA4, 0x01,
+	0xE8, 0x1B, 0xE3, 0x90, 0xF0, 0x18, 0xA4, 0x01,
+	0xE8, 0xBF, 0x8D, 0xB8, 0x4B, 0xD1, 0x4B, 0xD8,
+	0x0B, 0xCB, 0x0B, 0xC2, 0xE1, 0x00, 0xE3, 0x02,
+	0xE3, 0x03, 0x52, 0xD3, 0x60, 0x59, 0xE6, 0x93,
+	0x0D, 0x22, 0x52, 0xD4, 0xE6, 0x93, 0x0D, 0x2A,
+	0xE3, 0x98, 0xE3, 0x90, 0xE1, 0x00, 0x02, 0x5D
+};
+
+static const unsigned char patch_block5[64] = {
+	0x02, 0x63, 0xE3, 0x02, 0xC8, 0x12, 0x02, 0xCA,
+	0xC8, 0x52, 0x02, 0xC2, 0x82, 0x68, 0xE3, 0x02,
+	0xC8, 0x14, 0x02, 0xCA, 0xC8, 0x90, 0x02, 0xC2,
+	0x0A, 0xD0, 0xC9, 0x93, 0x0A, 0xDA, 0xCC, 0xD2,
+	0x0A, 0xE2, 0x63, 0x12, 0x02, 0xDA, 0x0A, 0x98,
+	0x0A, 0xA0, 0x0A, 0xA8, 0xE3, 0x90, 0xE1, 0x00,
+	0xE3, 0x02, 0x0A, 0xD0, 0xC9, 0x93, 0x0A, 0xDA,
+	0xCC, 0xD2, 0x0A, 0xE2, 0x63, 0x12, 0x02, 0xDA
+};
+
+static const unsigned char patch_block6[64] = {
+	0x0A, 0x98, 0x0A, 0xA0, 0x0A, 0xA8, 0x49, 0x91,
+	0xE5, 0x6A, 0xA4, 0x04, 0xC8, 0x12, 0x02, 0xCA,
+	0xC8, 0x52, 0x82, 0x89, 0xC8, 0x14, 0x02, 0xCA,
+	0xC8, 0x90, 0x02, 0xC2, 0xE3, 0x90, 0xE1, 0x00,
+	0x08, 0x60, 0xE1, 0x00, 0x48, 0x53, 0xE8, 0x97,
+	0x08, 0x5A, 0xE1, 0x00, 0xE3, 0x02, 0xE3, 0x03,
+	0x54, 0xD3, 0x60, 0x59, 0xE6, 0x93, 0x0D, 0x52,
+	0xE3, 0x98, 0xE3, 0x90, 0xE1, 0x00, 0x02, 0x9C
+};
+
+static const unsigned char patch_block7[64] = {
+	0xE3, 0x02, 0x55, 0x13, 0x93, 0x17, 0x55, 0x13,
+	0x93, 0x17, 0xE3, 0x90, 0xE1, 0x00, 0x75, 0x30,
+	0xE3, 0x02, 0xE3, 0x03, 0x55, 0x55, 0x60, 0x59,
+	0xE6, 0x93, 0x0D, 0xB2, 0xE3, 0x98, 0xE3, 0x90,
+	0xE1, 0x00, 0x02, 0xAE, 0xE7, 0x92, 0xE9, 0x18,
+	0xEA, 0x9A, 0xE8, 0x98, 0xE8, 0x10, 0xE8, 0x11,
+	0xE8, 0x51, 0xD2, 0xDA, 0xD2, 0xF3, 0xE8, 0x13,
+	0xD2, 0xFA, 0xE8, 0x50, 0xD2, 0xEA, 0xE8, 0xD0
+};
+
+static const unsigned char patch_block8[64] = {
+	0xE8, 0xD1, 0xD3, 0x0A, 0x03, 0x09, 0x48, 0x23,
+	0xE5, 0x2C, 0xA0, 0x03, 0x48, 0x24, 0xEA, 0x1C,
+	0x03, 0x08, 0xD2, 0xE3, 0xD3, 0x03, 0xD3, 0x13,
+	0xE1, 0x00, 0x02, 0xCB, 0x05, 0x93, 0x57, 0x93,
+	0xF0, 0x9A, 0xAC, 0x0B, 0xE3, 0x07, 0x92, 0xEA,
+	0xE2, 0x9F, 0xE5, 0x06, 0xE3, 0xB0, 0xA0, 0x02,
+	0xEB, 0x1E, 0x82, 0xD7, 0xEA, 0x1E, 0xE2, 0x3B,
+	0x85, 0x9B, 0xE9, 0x1E, 0xC8, 0x90, 0x85, 0x94
+};
+
+static const unsigned char patch_block9[64] = {
+	0x02, 0xDE, 0x05, 0x80, 0x57, 0x93, 0xF0, 0xBA,
+	0xAC, 0x06, 0x92, 0xEA, 0xE2, 0xBF, 0xE5, 0x06,
+	0xA0, 0x01, 0xEB, 0xBF, 0x85, 0x88, 0xE9, 0x3E,
+	0xC8, 0x90, 0x85, 0x81, 0xE9, 0x3E, 0xF0, 0xBA,
+	0xF3, 0x39, 0xF0, 0x3A, 0x60, 0x17, 0xF0, 0x3A,
+	0xC0, 0x90, 0xF0, 0xBA, 0xE1, 0x00, 0x00, 0x3F,
+	0xE3, 0x02, 0xE3, 0x03, 0x58, 0x10, 0x60, 0x59,
+	0xE6, 0x93, 0x0D, 0xA2, 0x58, 0x12, 0xE6, 0x93
+};
+
+static const unsigned char patch_block10[64] = {
+	0x0D, 0xAA, 0xE3, 0x98, 0xE3, 0x90, 0xE1, 0x00,
+	0x03, 0x01, 0xE1, 0x00, 0x03, 0x03, 0x9B, 0x7D,
+	0x8B, 0x8B, 0xE3, 0x02, 0xE3, 0x03, 0x58, 0x56,
+	0x60, 0x59, 0xE6, 0x93, 0x0D, 0xBA, 0xE3, 0x98,
+	0xE3, 0x90, 0xE1, 0x00, 0x03, 0x0F, 0x93, 0x11,
+	0xE1, 0x00, 0xE3, 0x02, 0x4A, 0x11, 0x0B, 0x42,
+	0x91, 0xAF, 0xE3, 0x90, 0xE1, 0x00, 0xF2, 0x91,
+	0xF0, 0x91, 0xA3, 0xFE, 0xE1, 0x00, 0x60, 0x92
+};
+
+static const unsigned char patch_block11[64] = {
+	0xC0, 0x5F, 0xF0, 0x13, 0xF0, 0x13, 0x59, 0x5B,
+	0xE2, 0x13, 0xF0, 0x11, 0x5A, 0x19, 0xE2, 0x13,
+	0xE1, 0x00, 0x00, 0x00, 0x03, 0x27, 0x68, 0x61,
+	0x76, 0x61, 0x6E, 0x61, 0x00, 0x06, 0x03, 0x2C,
+	0xE3, 0x02, 0xE3, 0x03, 0xE9, 0x38, 0x59, 0x15,
+	0x59, 0x5A, 0xF2, 0x9A, 0xBC, 0x0B, 0xA4, 0x0A,
+	0x59, 0x1E, 0xF3, 0x11, 0xF0, 0x1A, 0xE2, 0xBB,
+	0x59, 0x15, 0xF0, 0x11, 0x19, 0x2A, 0xE5, 0x02
+};
+
+static const unsigned char patch_block12[54] = {
+	0xA4, 0x01, 0xEB, 0xBF, 0xE3, 0x98, 0xE3, 0x90,
+	0xE1, 0x00, 0x03, 0x42, 0x19, 0x28, 0xE1, 0x00,
+	0xE9, 0x30, 0x60, 0x79, 0xE1, 0x00, 0xE3, 0x03,
+	0xE3, 0x07, 0x60, 0x79, 0x93, 0x4E, 0xE3, 0xB8,
+	0xE3, 0x98, 0xE1, 0x00, 0xE9, 0x1A, 0xF0, 0x1F,
+	0xE2, 0x33, 0xF0, 0x91, 0xE2, 0x92, 0xE0, 0x32,
+	0xF0, 0x31, 0xE1, 0x00, 0x00, 0x00
+};
+
+static const unsigned char do_call[1] = {
+	0x01
+};
+
+
+#define PATCH_DATA_SIZE 18
+
+static const cpia2_patch patch_data[PATCH_DATA_SIZE] = {
+	{0x0A, sizeof(start_address_hi), start_address_hi}
+	,			// 0
+	{0x0B, sizeof(start_address_lo), start_address_lo}
+	,			// 1
+	{0x0C, sizeof(patch_block0), patch_block0}
+	,			// 2
+	{0x0C, sizeof(patch_block1), patch_block1}
+	,			// 3
+	{0x0C, sizeof(patch_block2), patch_block2}
+	,			// 4
+	{0x0C, sizeof(patch_block3), patch_block3}
+	,			// 5
+	{0x0C, sizeof(patch_block4), patch_block4}
+	,			// 6
+	{0x0C, sizeof(patch_block5), patch_block5}
+	,			// 7
+	{0x0C, sizeof(patch_block6), patch_block6}
+	,			// 8
+	{0x0C, sizeof(patch_block7), patch_block7}
+	,			// 9
+	{0x0C, sizeof(patch_block8), patch_block8}
+	,			// 10
+	{0x0C, sizeof(patch_block9), patch_block9}
+	,			//11
+	{0x0C, sizeof(patch_block10), patch_block10}
+	,			// 12
+	{0x0C, sizeof(patch_block11), patch_block11}
+	,			// 13
+	{0x0C, sizeof(patch_block12), patch_block12}
+	,			// 14
+	{0x0A, sizeof(start_address_hi), start_address_hi}
+	,			// 15
+	{0x0B, sizeof(start_address_lo), start_address_lo}
+	,			// 16
+	{0x0D, sizeof(do_call), do_call}	//17
+};
+
+
+#endif