V4L/DVB (3376): Add cpia2 camera support
authorAlan Cox <alan@redhat.com>
Mon, 27 Feb 2006 03:09:05 +0000 (00:09 -0300)
committerMauro Carvalho Chehab <mchehab@infradead.org>
Mon, 27 Feb 2006 03:09:05 +0000 (00:09 -0300)
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:
Documentation/video4linux/README.cpia2 [new file with mode: 0644]
Documentation/video4linux/cpia2_overview.txt [new file with mode: 0644]
drivers/media/video/Kconfig
drivers/media/video/Makefile
drivers/media/video/cpia2/Makefile [new file with mode: 0644]
drivers/media/video/cpia2/cpia2.h [new file with mode: 0644]
drivers/media/video/cpia2/cpia2_core.c [new file with mode: 0644]
drivers/media/video/cpia2/cpia2_registers.h [new file with mode: 0644]
drivers/media/video/cpia2/cpia2_usb.c [new file with mode: 0644]
drivers/media/video/cpia2/cpia2_v4l.c [new file with mode: 0644]
drivers/media/video/cpia2/cpia2dev.h [new file with mode: 0644]
drivers/media/video/cpia2/cpia2patch.h [new file with mode: 0644]

diff --git a/Documentation/video4linux/README.cpia2 b/Documentation/video4linux/README.cpia2
new file mode 100644 (file)
index 0000000..f3bd343
--- /dev/null
@@ -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 (file)
index 0000000..a6e5366
--- /dev/null
@@ -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
index d82c8a30ba44373e137ba43b7fe8f0067d156119..f6889f771053d126e3e56868aa5002de9317a362 100644 (file)
@@ -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
index faf728366c4e4e57655939d8540b2908b8fd1691..87b1ce6e2050a89c34841b30e174e66ab3efba77 100644 (file)
@@ -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 (file)
index 0000000..828cf1b
--- /dev/null
@@ -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 (file)
index 0000000..95d3afa
--- /dev/null
@@ -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 (file)
index 0000000..5dfb242
--- /dev/null
@@ -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 (file)
index 0000000..3bbec51
--- /dev/null
@@ -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 (file)
index 0000000..f4da029
--- /dev/null
@@ -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 (file)
index 0000000..3480a2c
--- /dev/null
@@ -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 (file)
index 0000000..d58097c
--- /dev/null
@@ -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 (file)
index 0000000..7f085fb
--- /dev/null
@@ -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