/linux-2.6.21.x/drivers/media/video/cpia2/cpia2_core.c

Large files files are truncated, but you can click here to view the full file

/****************************************************************************
 *
 *  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
 *
 *******************************…