cachepc-linux

Fork of AMDESE/linux with modifications for CachePC side-channel attack
git clone https://git.sinitax.com/sinitax/cachepc-linux
Log | Files | Refs | README | LICENSE | sfeed.txt

cpia2_core.c (73672B)


      1// SPDX-License-Identifier: GPL-2.0-or-later
      2/****************************************************************************
      3 *
      4 *  Filename: cpia2_core.c
      5 *
      6 *  Copyright 2001, STMicrolectronics, Inc.
      7 *      Contact:  steve.miller@st.com
      8 *
      9 *  Description:
     10 *     This is a USB driver for CPia2 based video cameras.
     11 *     The infrastructure of this driver is based on the cpia usb driver by
     12 *     Jochen Scharrlach and Johannes Erdfeldt.
     13 *
     14 *  Stripped of 2.4 stuff ready for main kernel submit by
     15 *		Alan Cox <alan@lxorguk.ukuu.org.uk>
     16 *
     17 ****************************************************************************/
     18
     19#include "cpia2.h"
     20
     21#include <linux/module.h>
     22#include <linux/slab.h>
     23#include <linux/mm.h>
     24#include <linux/vmalloc.h>
     25#include <linux/firmware.h>
     26#include <linux/sched/signal.h>
     27
     28#define FIRMWARE "cpia2/stv0672_vp4.bin"
     29MODULE_FIRMWARE(FIRMWARE);
     30
     31/* #define _CPIA2_DEBUG_ */
     32
     33#ifdef _CPIA2_DEBUG_
     34
     35static const char *block_name[] = {
     36	"System",
     37	"VC",
     38	"VP",
     39	"IDATA"
     40};
     41#endif
     42
     43static unsigned int debugs_on;	/* default 0 - DEBUG_REG */
     44
     45
     46/******************************************************************************
     47 *
     48 *  Forward Declarations
     49 *
     50 *****************************************************************************/
     51static int apply_vp_patch(struct camera_data *cam);
     52static int set_default_user_mode(struct camera_data *cam);
     53static int set_vw_size(struct camera_data *cam, int size);
     54static int configure_sensor(struct camera_data *cam,
     55			    int reqwidth, int reqheight);
     56static int config_sensor_410(struct camera_data *cam,
     57			    int reqwidth, int reqheight);
     58static int config_sensor_500(struct camera_data *cam,
     59			    int reqwidth, int reqheight);
     60static int set_all_properties(struct camera_data *cam);
     61static void wake_system(struct camera_data *cam);
     62static void set_lowlight_boost(struct camera_data *cam);
     63static void reset_camera_struct(struct camera_data *cam);
     64static int cpia2_set_high_power(struct camera_data *cam);
     65
     66/* Here we want the physical address of the memory.
     67 * This is used when initializing the contents of the
     68 * area and marking the pages as reserved.
     69 */
     70static inline unsigned long kvirt_to_pa(unsigned long adr)
     71{
     72	unsigned long kva, ret;
     73
     74	kva = (unsigned long) page_address(vmalloc_to_page((void *)adr));
     75	kva |= adr & (PAGE_SIZE-1); /* restore the offset */
     76	ret = __pa(kva);
     77	return ret;
     78}
     79
     80static void *rvmalloc(unsigned long size)
     81{
     82	void *mem;
     83	unsigned long adr;
     84
     85	/* Round it off to PAGE_SIZE */
     86	size = PAGE_ALIGN(size);
     87
     88	mem = vmalloc_32(size);
     89	if (!mem)
     90		return NULL;
     91
     92	memset(mem, 0, size);	/* Clear the ram out, no junk to the user */
     93	adr = (unsigned long) mem;
     94
     95	while ((long)size > 0) {
     96		SetPageReserved(vmalloc_to_page((void *)adr));
     97		adr += PAGE_SIZE;
     98		size -= PAGE_SIZE;
     99	}
    100	return mem;
    101}
    102
    103static void rvfree(void *mem, unsigned long size)
    104{
    105	unsigned long adr;
    106
    107	if (!mem)
    108		return;
    109
    110	size = PAGE_ALIGN(size);
    111
    112	adr = (unsigned long) mem;
    113	while ((long)size > 0) {
    114		ClearPageReserved(vmalloc_to_page((void *)adr));
    115		adr += PAGE_SIZE;
    116		size -= PAGE_SIZE;
    117	}
    118	vfree(mem);
    119}
    120
    121/******************************************************************************
    122 *
    123 *  cpia2_do_command
    124 *
    125 *  Send an arbitrary command to the camera.  For commands that read from
    126 *  the camera, copy the buffers into the proper param structures.
    127 *****************************************************************************/
    128int cpia2_do_command(struct camera_data *cam,
    129		     u32 command, u8 direction, u8 param)
    130{
    131	int retval = 0;
    132	struct cpia2_command cmd;
    133	unsigned int device = cam->params.pnp_id.device_type;
    134
    135	cmd.command = command;
    136	cmd.reg_count = 2;	/* default */
    137	cmd.direction = direction;
    138
    139	/***
    140	 * Set up the command.
    141	 ***/
    142	switch (command) {
    143	case CPIA2_CMD_GET_VERSION:
    144		cmd.req_mode =
    145		    CAMERAACCESS_TYPE_BLOCK | CAMERAACCESS_SYSTEM;
    146		cmd.start = CPIA2_SYSTEM_DEVICE_HI;
    147		break;
    148	case CPIA2_CMD_GET_PNP_ID:
    149		cmd.req_mode =
    150		    CAMERAACCESS_TYPE_BLOCK | CAMERAACCESS_SYSTEM;
    151		cmd.reg_count = 8;
    152		cmd.start = CPIA2_SYSTEM_DESCRIP_VID_HI;
    153		break;
    154	case CPIA2_CMD_GET_ASIC_TYPE:
    155		cmd.req_mode = CAMERAACCESS_TYPE_BLOCK | CAMERAACCESS_VC;
    156		cmd.start = CPIA2_VC_ASIC_ID;
    157		break;
    158	case CPIA2_CMD_GET_SENSOR:
    159		cmd.req_mode = CAMERAACCESS_TYPE_BLOCK | CAMERAACCESS_VP;
    160		cmd.start = CPIA2_VP_SENSOR_FLAGS;
    161		break;
    162	case CPIA2_CMD_GET_VP_DEVICE:
    163		cmd.req_mode = CAMERAACCESS_TYPE_BLOCK | CAMERAACCESS_VP;
    164		cmd.start = CPIA2_VP_DEVICEH;
    165		break;
    166	case CPIA2_CMD_SET_VP_BRIGHTNESS:
    167		cmd.buffer.block_data[0] = param;
    168		fallthrough;
    169	case CPIA2_CMD_GET_VP_BRIGHTNESS:
    170		cmd.req_mode = CAMERAACCESS_TYPE_BLOCK | CAMERAACCESS_VP;
    171		cmd.reg_count = 1;
    172		if (device == DEVICE_STV_672)
    173			cmd.start = CPIA2_VP4_EXPOSURE_TARGET;
    174		else
    175			cmd.start = CPIA2_VP5_EXPOSURE_TARGET;
    176		break;
    177	case CPIA2_CMD_SET_CONTRAST:
    178		cmd.buffer.block_data[0] = param;
    179		fallthrough;
    180	case CPIA2_CMD_GET_CONTRAST:
    181		cmd.req_mode = CAMERAACCESS_TYPE_BLOCK | CAMERAACCESS_VP;
    182		cmd.reg_count = 1;
    183		cmd.start = CPIA2_VP_YRANGE;
    184		break;
    185	case CPIA2_CMD_SET_VP_SATURATION:
    186		cmd.buffer.block_data[0] = param;
    187		fallthrough;
    188	case CPIA2_CMD_GET_VP_SATURATION:
    189		cmd.req_mode = CAMERAACCESS_TYPE_BLOCK | CAMERAACCESS_VP;
    190		cmd.reg_count = 1;
    191		if (device == DEVICE_STV_672)
    192			cmd.start = CPIA2_VP_SATURATION;
    193		else
    194			cmd.start = CPIA2_VP5_MCUVSATURATION;
    195		break;
    196	case CPIA2_CMD_SET_VP_GPIO_DATA:
    197		cmd.buffer.block_data[0] = param;
    198		fallthrough;
    199	case CPIA2_CMD_GET_VP_GPIO_DATA:
    200		cmd.req_mode = CAMERAACCESS_TYPE_BLOCK | CAMERAACCESS_VP;
    201		cmd.reg_count = 1;
    202		cmd.start = CPIA2_VP_GPIO_DATA;
    203		break;
    204	case CPIA2_CMD_SET_VP_GPIO_DIRECTION:
    205		cmd.buffer.block_data[0] = param;
    206		fallthrough;
    207	case CPIA2_CMD_GET_VP_GPIO_DIRECTION:
    208		cmd.req_mode = CAMERAACCESS_TYPE_BLOCK | CAMERAACCESS_VP;
    209		cmd.reg_count = 1;
    210		cmd.start = CPIA2_VP_GPIO_DIRECTION;
    211		break;
    212	case CPIA2_CMD_SET_VC_MP_GPIO_DATA:
    213		cmd.buffer.block_data[0] = param;
    214		fallthrough;
    215	case CPIA2_CMD_GET_VC_MP_GPIO_DATA:
    216		cmd.req_mode = CAMERAACCESS_TYPE_BLOCK | CAMERAACCESS_VC;
    217		cmd.reg_count = 1;
    218		cmd.start = CPIA2_VC_MP_DATA;
    219		break;
    220	case CPIA2_CMD_SET_VC_MP_GPIO_DIRECTION:
    221		cmd.buffer.block_data[0] = param;
    222		fallthrough;
    223	case CPIA2_CMD_GET_VC_MP_GPIO_DIRECTION:
    224		cmd.req_mode = CAMERAACCESS_TYPE_BLOCK | CAMERAACCESS_VC;
    225		cmd.reg_count = 1;
    226		cmd.start = CPIA2_VC_MP_DIR;
    227		break;
    228	case CPIA2_CMD_ENABLE_PACKET_CTRL:
    229		cmd.req_mode =
    230		    CAMERAACCESS_TYPE_BLOCK | CAMERAACCESS_SYSTEM;
    231		cmd.start = CPIA2_SYSTEM_INT_PACKET_CTRL;
    232		cmd.reg_count = 1;
    233		cmd.buffer.block_data[0] = param;
    234		break;
    235	case CPIA2_CMD_SET_FLICKER_MODES:
    236		cmd.buffer.block_data[0] = param;
    237		fallthrough;
    238	case CPIA2_CMD_GET_FLICKER_MODES:
    239		cmd.req_mode = CAMERAACCESS_TYPE_BLOCK | CAMERAACCESS_VP;
    240		cmd.reg_count = 1;
    241		cmd.start = CPIA2_VP_FLICKER_MODES;
    242		break;
    243	case CPIA2_CMD_RESET_FIFO:	/* clear fifo and enable stream block */
    244		cmd.req_mode = CAMERAACCESS_TYPE_RANDOM | CAMERAACCESS_VC;
    245		cmd.reg_count = 2;
    246		cmd.start = 0;
    247		cmd.buffer.registers[0].index = CPIA2_VC_ST_CTRL;
    248		cmd.buffer.registers[0].value = CPIA2_VC_ST_CTRL_SRC_VC |
    249		    CPIA2_VC_ST_CTRL_DST_USB | CPIA2_VC_ST_CTRL_EOF_DETECT;
    250		cmd.buffer.registers[1].index = CPIA2_VC_ST_CTRL;
    251		cmd.buffer.registers[1].value = CPIA2_VC_ST_CTRL_SRC_VC |
    252		    CPIA2_VC_ST_CTRL_DST_USB |
    253		    CPIA2_VC_ST_CTRL_EOF_DETECT |
    254		    CPIA2_VC_ST_CTRL_FIFO_ENABLE;
    255		break;
    256	case CPIA2_CMD_SET_HI_POWER:
    257		cmd.req_mode =
    258		    CAMERAACCESS_TYPE_RANDOM | CAMERAACCESS_SYSTEM;
    259		cmd.reg_count = 2;
    260		cmd.buffer.registers[0].index =
    261		    CPIA2_SYSTEM_SYSTEM_CONTROL;
    262		cmd.buffer.registers[1].index =
    263		    CPIA2_SYSTEM_SYSTEM_CONTROL;
    264		cmd.buffer.registers[0].value = CPIA2_SYSTEM_CONTROL_CLEAR_ERR;
    265		cmd.buffer.registers[1].value =
    266		    CPIA2_SYSTEM_CONTROL_HIGH_POWER;
    267		break;
    268	case CPIA2_CMD_SET_LOW_POWER:
    269		cmd.req_mode =
    270		    CAMERAACCESS_TYPE_BLOCK | CAMERAACCESS_SYSTEM;
    271		cmd.reg_count = 1;
    272		cmd.start = CPIA2_SYSTEM_SYSTEM_CONTROL;
    273		cmd.buffer.block_data[0] = 0;
    274		break;
    275	case CPIA2_CMD_CLEAR_V2W_ERR:
    276		cmd.req_mode =
    277		    CAMERAACCESS_TYPE_BLOCK | CAMERAACCESS_SYSTEM;
    278		cmd.reg_count = 1;
    279		cmd.start = CPIA2_SYSTEM_SYSTEM_CONTROL;
    280		cmd.buffer.block_data[0] = CPIA2_SYSTEM_CONTROL_CLEAR_ERR;
    281		break;
    282	case CPIA2_CMD_SET_USER_MODE:
    283		cmd.buffer.block_data[0] = param;
    284		fallthrough;
    285	case CPIA2_CMD_GET_USER_MODE:
    286		cmd.req_mode = CAMERAACCESS_TYPE_BLOCK | CAMERAACCESS_VP;
    287		cmd.reg_count = 1;
    288		if (device == DEVICE_STV_672)
    289			cmd.start = CPIA2_VP4_USER_MODE;
    290		else
    291			cmd.start = CPIA2_VP5_USER_MODE;
    292		break;
    293	case CPIA2_CMD_FRAMERATE_REQ:
    294		cmd.req_mode = CAMERAACCESS_TYPE_BLOCK | CAMERAACCESS_VP;
    295		cmd.reg_count = 1;
    296		if (device == DEVICE_STV_672)
    297			cmd.start = CPIA2_VP4_FRAMERATE_REQUEST;
    298		else
    299			cmd.start = CPIA2_VP5_FRAMERATE_REQUEST;
    300		cmd.buffer.block_data[0] = param;
    301		break;
    302	case CPIA2_CMD_SET_WAKEUP:
    303		cmd.buffer.block_data[0] = param;
    304		fallthrough;
    305	case CPIA2_CMD_GET_WAKEUP:
    306		cmd.req_mode = CAMERAACCESS_TYPE_BLOCK | CAMERAACCESS_VC;
    307		cmd.reg_count = 1;
    308		cmd.start = CPIA2_VC_WAKEUP;
    309		break;
    310	case CPIA2_CMD_SET_PW_CONTROL:
    311		cmd.buffer.block_data[0] = param;
    312		fallthrough;
    313	case CPIA2_CMD_GET_PW_CONTROL:
    314		cmd.req_mode = CAMERAACCESS_TYPE_BLOCK | CAMERAACCESS_VC;
    315		cmd.reg_count = 1;
    316		cmd.start = CPIA2_VC_PW_CTRL;
    317		break;
    318	case CPIA2_CMD_GET_VP_SYSTEM_STATE:
    319		cmd.req_mode = CAMERAACCESS_TYPE_BLOCK | CAMERAACCESS_VP;
    320		cmd.reg_count = 1;
    321		cmd.start = CPIA2_VP_SYSTEMSTATE;
    322		break;
    323	case CPIA2_CMD_SET_SYSTEM_CTRL:
    324		cmd.buffer.block_data[0] = param;
    325		fallthrough;
    326	case CPIA2_CMD_GET_SYSTEM_CTRL:
    327		cmd.req_mode =
    328		    CAMERAACCESS_TYPE_BLOCK | CAMERAACCESS_SYSTEM;
    329		cmd.reg_count = 1;
    330		cmd.start = CPIA2_SYSTEM_SYSTEM_CONTROL;
    331		break;
    332	case CPIA2_CMD_SET_VP_SYSTEM_CTRL:
    333		cmd.buffer.block_data[0] = param;
    334		fallthrough;
    335	case CPIA2_CMD_GET_VP_SYSTEM_CTRL:
    336		cmd.req_mode = CAMERAACCESS_TYPE_BLOCK | CAMERAACCESS_VP;
    337		cmd.reg_count = 1;
    338		cmd.start = CPIA2_VP_SYSTEMCTRL;
    339		break;
    340	case CPIA2_CMD_SET_VP_EXP_MODES:
    341		cmd.buffer.block_data[0] = param;
    342		fallthrough;
    343	case CPIA2_CMD_GET_VP_EXP_MODES:
    344		cmd.req_mode = CAMERAACCESS_TYPE_BLOCK | CAMERAACCESS_VP;
    345		cmd.reg_count = 1;
    346		cmd.start = CPIA2_VP_EXPOSURE_MODES;
    347		break;
    348	case CPIA2_CMD_SET_DEVICE_CONFIG:
    349		cmd.buffer.block_data[0] = param;
    350		fallthrough;
    351	case CPIA2_CMD_GET_DEVICE_CONFIG:
    352		cmd.req_mode = CAMERAACCESS_TYPE_BLOCK | CAMERAACCESS_VP;
    353		cmd.reg_count = 1;
    354		cmd.start = CPIA2_VP_DEVICE_CONFIG;
    355		break;
    356	case CPIA2_CMD_SET_SERIAL_ADDR:
    357		cmd.buffer.block_data[0] = param;
    358		cmd.req_mode =
    359		    CAMERAACCESS_TYPE_BLOCK | CAMERAACCESS_SYSTEM;
    360		cmd.reg_count = 1;
    361		cmd.start = CPIA2_SYSTEM_VP_SERIAL_ADDR;
    362		break;
    363	case CPIA2_CMD_SET_SENSOR_CR1:
    364		cmd.buffer.block_data[0] = param;
    365		cmd.req_mode = CAMERAACCESS_TYPE_BLOCK | CAMERAACCESS_VP;
    366		cmd.reg_count = 1;
    367		cmd.start = CPIA2_SENSOR_CR1;
    368		break;
    369	case CPIA2_CMD_SET_VC_CONTROL:
    370		cmd.buffer.block_data[0] = param;
    371		fallthrough;
    372	case CPIA2_CMD_GET_VC_CONTROL:
    373		cmd.req_mode = CAMERAACCESS_TYPE_BLOCK | CAMERAACCESS_VC;
    374		cmd.reg_count = 1;
    375		cmd.start = CPIA2_VC_VC_CTRL;
    376		break;
    377	case CPIA2_CMD_SET_TARGET_KB:
    378		cmd.req_mode = CAMERAACCESS_TYPE_RANDOM | CAMERAACCESS_VC;
    379		cmd.reg_count = 1;
    380		cmd.buffer.registers[0].index = CPIA2_VC_VC_TARGET_KB;
    381		cmd.buffer.registers[0].value = param;
    382		break;
    383	case CPIA2_CMD_SET_DEF_JPEG_OPT:
    384		cmd.req_mode = CAMERAACCESS_TYPE_RANDOM | CAMERAACCESS_VC;
    385		cmd.reg_count = 4;
    386		cmd.buffer.registers[0].index = CPIA2_VC_VC_JPEG_OPT;
    387		cmd.buffer.registers[0].value =
    388		    CPIA2_VC_VC_JPEG_OPT_DOUBLE_SQUEEZE;
    389		cmd.buffer.registers[1].index = CPIA2_VC_VC_USER_SQUEEZE;
    390		cmd.buffer.registers[1].value = 20;
    391		cmd.buffer.registers[2].index = CPIA2_VC_VC_CREEP_PERIOD;
    392		cmd.buffer.registers[2].value = 2;
    393		cmd.buffer.registers[3].index = CPIA2_VC_VC_JPEG_OPT;
    394		cmd.buffer.registers[3].value = CPIA2_VC_VC_JPEG_OPT_DEFAULT;
    395		break;
    396	case CPIA2_CMD_REHASH_VP4:
    397		cmd.req_mode = CAMERAACCESS_TYPE_BLOCK | CAMERAACCESS_VP;
    398		cmd.reg_count = 1;
    399		cmd.start = CPIA2_VP_REHASH_VALUES;
    400		cmd.buffer.block_data[0] = param;
    401		break;
    402	case CPIA2_CMD_SET_USER_EFFECTS:  /* Note: Be careful with this as
    403					     this register can also affect
    404					     flicker modes */
    405		cmd.buffer.block_data[0] = param;
    406		fallthrough;
    407	case CPIA2_CMD_GET_USER_EFFECTS:
    408		cmd.req_mode = CAMERAACCESS_TYPE_BLOCK | CAMERAACCESS_VP;
    409		cmd.reg_count = 1;
    410		if (device == DEVICE_STV_672)
    411			cmd.start = CPIA2_VP4_USER_EFFECTS;
    412		else
    413			cmd.start = CPIA2_VP5_USER_EFFECTS;
    414		break;
    415	default:
    416		LOG("DoCommand received invalid command\n");
    417		return -EINVAL;
    418	}
    419
    420	retval = cpia2_send_command(cam, &cmd);
    421	if (retval) {
    422		return retval;
    423	}
    424
    425	/***
    426	 * Now copy any results from a read into the appropriate param struct.
    427	 ***/
    428	switch (command) {
    429	case CPIA2_CMD_GET_VERSION:
    430		cam->params.version.firmware_revision_hi =
    431		    cmd.buffer.block_data[0];
    432		cam->params.version.firmware_revision_lo =
    433		    cmd.buffer.block_data[1];
    434		break;
    435	case CPIA2_CMD_GET_PNP_ID:
    436		cam->params.pnp_id.vendor = (cmd.buffer.block_data[0] << 8) |
    437					    cmd.buffer.block_data[1];
    438		cam->params.pnp_id.product = (cmd.buffer.block_data[2] << 8) |
    439					     cmd.buffer.block_data[3];
    440		cam->params.pnp_id.device_revision =
    441			(cmd.buffer.block_data[4] << 8) |
    442			cmd.buffer.block_data[5];
    443		if (cam->params.pnp_id.vendor == 0x553) {
    444			if (cam->params.pnp_id.product == 0x100) {
    445				cam->params.pnp_id.device_type = DEVICE_STV_672;
    446			} else if (cam->params.pnp_id.product == 0x140 ||
    447				   cam->params.pnp_id.product == 0x151) {
    448				cam->params.pnp_id.device_type = DEVICE_STV_676;
    449			}
    450		}
    451		break;
    452	case CPIA2_CMD_GET_ASIC_TYPE:
    453		cam->params.version.asic_id = cmd.buffer.block_data[0];
    454		cam->params.version.asic_rev = cmd.buffer.block_data[1];
    455		break;
    456	case CPIA2_CMD_GET_SENSOR:
    457		cam->params.version.sensor_flags = cmd.buffer.block_data[0];
    458		cam->params.version.sensor_rev = cmd.buffer.block_data[1];
    459		break;
    460	case CPIA2_CMD_GET_VP_DEVICE:
    461		cam->params.version.vp_device_hi = cmd.buffer.block_data[0];
    462		cam->params.version.vp_device_lo = cmd.buffer.block_data[1];
    463		break;
    464	case CPIA2_CMD_GET_VP_GPIO_DATA:
    465		cam->params.vp_params.gpio_data = cmd.buffer.block_data[0];
    466		break;
    467	case CPIA2_CMD_GET_VP_GPIO_DIRECTION:
    468		cam->params.vp_params.gpio_direction = cmd.buffer.block_data[0];
    469		break;
    470	case CPIA2_CMD_GET_VC_MP_GPIO_DIRECTION:
    471		cam->params.vc_params.vc_mp_direction =cmd.buffer.block_data[0];
    472		break;
    473	case CPIA2_CMD_GET_VC_MP_GPIO_DATA:
    474		cam->params.vc_params.vc_mp_data = cmd.buffer.block_data[0];
    475		break;
    476	case CPIA2_CMD_GET_FLICKER_MODES:
    477		cam->params.flicker_control.cam_register =
    478			cmd.buffer.block_data[0];
    479		break;
    480	case CPIA2_CMD_GET_WAKEUP:
    481		cam->params.vc_params.wakeup = cmd.buffer.block_data[0];
    482		break;
    483	case CPIA2_CMD_GET_PW_CONTROL:
    484		cam->params.vc_params.pw_control = cmd.buffer.block_data[0];
    485		break;
    486	case CPIA2_CMD_GET_SYSTEM_CTRL:
    487		cam->params.camera_state.system_ctrl = cmd.buffer.block_data[0];
    488		break;
    489	case CPIA2_CMD_GET_VP_SYSTEM_STATE:
    490		cam->params.vp_params.system_state = cmd.buffer.block_data[0];
    491		break;
    492	case CPIA2_CMD_GET_VP_SYSTEM_CTRL:
    493		cam->params.vp_params.system_ctrl = cmd.buffer.block_data[0];
    494		break;
    495	case CPIA2_CMD_GET_VP_EXP_MODES:
    496		cam->params.vp_params.exposure_modes = cmd.buffer.block_data[0];
    497		break;
    498	case CPIA2_CMD_GET_DEVICE_CONFIG:
    499		cam->params.vp_params.device_config = cmd.buffer.block_data[0];
    500		break;
    501	case CPIA2_CMD_GET_VC_CONTROL:
    502		cam->params.vc_params.vc_control = cmd.buffer.block_data[0];
    503		break;
    504	case CPIA2_CMD_GET_USER_MODE:
    505		cam->params.vp_params.video_mode = cmd.buffer.block_data[0];
    506		break;
    507	case CPIA2_CMD_GET_USER_EFFECTS:
    508		cam->params.vp_params.user_effects = cmd.buffer.block_data[0];
    509		break;
    510	default:
    511		break;
    512	}
    513	return retval;
    514}
    515
    516/******************************************************************************
    517 *
    518 *  cpia2_send_command
    519 *
    520 *****************************************************************************/
    521
    522#define DIR(cmd) ((cmd->direction == TRANSFER_WRITE) ? "Write" : "Read")
    523#define BINDEX(cmd) (cmd->req_mode & 0x03)
    524
    525int cpia2_send_command(struct camera_data *cam, struct cpia2_command *cmd)
    526{
    527	u8 count;
    528	u8 start;
    529	u8 *buffer;
    530	int retval;
    531
    532	switch (cmd->req_mode & 0x0c) {
    533	case CAMERAACCESS_TYPE_RANDOM:
    534		count = cmd->reg_count * sizeof(struct cpia2_register);
    535		start = 0;
    536		buffer = (u8 *) & cmd->buffer;
    537		if (debugs_on & DEBUG_REG)
    538			DBG("%s Random: Register block %s\n", DIR(cmd),
    539			    block_name[BINDEX(cmd)]);
    540		break;
    541	case CAMERAACCESS_TYPE_BLOCK:
    542		count = cmd->reg_count;
    543		start = cmd->start;
    544		buffer = cmd->buffer.block_data;
    545		if (debugs_on & DEBUG_REG)
    546			DBG("%s Block: Register block %s\n", DIR(cmd),
    547			    block_name[BINDEX(cmd)]);
    548		break;
    549	case CAMERAACCESS_TYPE_MASK:
    550		count = cmd->reg_count * sizeof(struct cpia2_reg_mask);
    551		start = 0;
    552		buffer = (u8 *) & cmd->buffer;
    553		if (debugs_on & DEBUG_REG)
    554			DBG("%s Mask: Register block %s\n", DIR(cmd),
    555			    block_name[BINDEX(cmd)]);
    556		break;
    557	case CAMERAACCESS_TYPE_REPEAT:	/* For patch blocks only */
    558		count = cmd->reg_count;
    559		start = cmd->start;
    560		buffer = cmd->buffer.block_data;
    561		if (debugs_on & DEBUG_REG)
    562			DBG("%s Repeat: Register block %s\n", DIR(cmd),
    563			    block_name[BINDEX(cmd)]);
    564		break;
    565	default:
    566		LOG("%s: invalid request mode\n",__func__);
    567		return -EINVAL;
    568	}
    569
    570	retval = cpia2_usb_transfer_cmd(cam,
    571					buffer,
    572					cmd->req_mode,
    573					start, count, cmd->direction);
    574#ifdef _CPIA2_DEBUG_
    575	if (debugs_on & DEBUG_REG) {
    576		int i;
    577		for (i = 0; i < cmd->reg_count; i++) {
    578			if((cmd->req_mode & 0x0c) == CAMERAACCESS_TYPE_BLOCK)
    579				KINFO("%s Block: [0x%02X] = 0x%02X\n",
    580				    DIR(cmd), start + i, buffer[i]);
    581			if((cmd->req_mode & 0x0c) == CAMERAACCESS_TYPE_RANDOM)
    582				KINFO("%s Random: [0x%02X] = 0x%02X\n",
    583				    DIR(cmd), cmd->buffer.registers[i].index,
    584				    cmd->buffer.registers[i].value);
    585		}
    586	}
    587#endif
    588
    589	return retval;
    590};
    591
    592/*************
    593 * Functions to implement camera functionality
    594 *************/
    595/******************************************************************************
    596 *
    597 *  cpia2_get_version_info
    598 *
    599 *****************************************************************************/
    600static void cpia2_get_version_info(struct camera_data *cam)
    601{
    602	cpia2_do_command(cam, CPIA2_CMD_GET_VERSION, TRANSFER_READ, 0);
    603	cpia2_do_command(cam, CPIA2_CMD_GET_PNP_ID, TRANSFER_READ, 0);
    604	cpia2_do_command(cam, CPIA2_CMD_GET_ASIC_TYPE, TRANSFER_READ, 0);
    605	cpia2_do_command(cam, CPIA2_CMD_GET_SENSOR, TRANSFER_READ, 0);
    606	cpia2_do_command(cam, CPIA2_CMD_GET_VP_DEVICE, TRANSFER_READ, 0);
    607}
    608
    609/******************************************************************************
    610 *
    611 *  cpia2_reset_camera
    612 *
    613 *  Called at least during the open process, sets up initial params.
    614 *****************************************************************************/
    615int cpia2_reset_camera(struct camera_data *cam)
    616{
    617	u8 tmp_reg;
    618	int retval = 0;
    619	int target_kb;
    620	int i;
    621	struct cpia2_command cmd;
    622
    623	/***
    624	 * VC setup
    625	 ***/
    626	retval = configure_sensor(cam,
    627				  cam->params.roi.width,
    628				  cam->params.roi.height);
    629	if (retval < 0) {
    630		ERR("Couldn't configure sensor, error=%d\n", retval);
    631		return retval;
    632	}
    633
    634	/* Clear FIFO and route/enable stream block */
    635	cmd.req_mode = CAMERAACCESS_TYPE_RANDOM | CAMERAACCESS_VC;
    636	cmd.direction = TRANSFER_WRITE;
    637	cmd.reg_count = 2;
    638	cmd.buffer.registers[0].index = CPIA2_VC_ST_CTRL;
    639	cmd.buffer.registers[0].value = CPIA2_VC_ST_CTRL_SRC_VC |
    640		CPIA2_VC_ST_CTRL_DST_USB | CPIA2_VC_ST_CTRL_EOF_DETECT;
    641	cmd.buffer.registers[1].index = CPIA2_VC_ST_CTRL;
    642	cmd.buffer.registers[1].value = CPIA2_VC_ST_CTRL_SRC_VC |
    643		CPIA2_VC_ST_CTRL_DST_USB |
    644		CPIA2_VC_ST_CTRL_EOF_DETECT | CPIA2_VC_ST_CTRL_FIFO_ENABLE;
    645
    646	cpia2_send_command(cam, &cmd);
    647
    648	cpia2_set_high_power(cam);
    649
    650	if (cam->params.pnp_id.device_type == DEVICE_STV_672) {
    651		/* Enable button notification */
    652		cmd.req_mode = CAMERAACCESS_TYPE_RANDOM | CAMERAACCESS_SYSTEM;
    653		cmd.buffer.registers[0].index = CPIA2_SYSTEM_INT_PACKET_CTRL;
    654		cmd.buffer.registers[0].value =
    655			CPIA2_SYSTEM_INT_PACKET_CTRL_ENABLE_SW_XX;
    656		cmd.reg_count = 1;
    657		cpia2_send_command(cam, &cmd);
    658	}
    659
    660	schedule_timeout_interruptible(msecs_to_jiffies(100));
    661
    662	if (cam->params.pnp_id.device_type == DEVICE_STV_672)
    663		retval = apply_vp_patch(cam);
    664
    665	/* wait for vp to go to sleep */
    666	schedule_timeout_interruptible(msecs_to_jiffies(100));
    667
    668	/***
    669	 * If this is a 676, apply VP5 fixes before we start streaming
    670	 ***/
    671	if (cam->params.pnp_id.device_type == DEVICE_STV_676) {
    672		cmd.req_mode = CAMERAACCESS_TYPE_RANDOM | CAMERAACCESS_VP;
    673
    674		/* The following writes improve the picture */
    675		cmd.buffer.registers[0].index = CPIA2_VP5_MYBLACK_LEVEL;
    676		cmd.buffer.registers[0].value = 0; /* reduce from the default
    677						    * rec 601 pedestal of 16 */
    678		cmd.buffer.registers[1].index = CPIA2_VP5_MCYRANGE;
    679		cmd.buffer.registers[1].value = 0x92; /* increase from 100% to
    680						       * (256/256 - 31) to fill
    681						       * available range */
    682		cmd.buffer.registers[2].index = CPIA2_VP5_MYCEILING;
    683		cmd.buffer.registers[2].value = 0xFF; /* Increase from the
    684						       * default rec 601 ceiling
    685						       * of 240 */
    686		cmd.buffer.registers[3].index = CPIA2_VP5_MCUVSATURATION;
    687		cmd.buffer.registers[3].value = 0xFF; /* Increase from the rec
    688						       * 601 100% level (128)
    689						       * to 145-192 */
    690		cmd.buffer.registers[4].index = CPIA2_VP5_ANTIFLKRSETUP;
    691		cmd.buffer.registers[4].value = 0x80;  /* Inhibit the
    692							* anti-flicker */
    693
    694		/* The following 4 writes are a fix to allow QVGA to work at 30 fps */
    695		cmd.buffer.registers[5].index = CPIA2_VP_RAM_ADDR_H;
    696		cmd.buffer.registers[5].value = 0x01;
    697		cmd.buffer.registers[6].index = CPIA2_VP_RAM_ADDR_L;
    698		cmd.buffer.registers[6].value = 0xE3;
    699		cmd.buffer.registers[7].index = CPIA2_VP_RAM_DATA;
    700		cmd.buffer.registers[7].value = 0x02;
    701		cmd.buffer.registers[8].index = CPIA2_VP_RAM_DATA;
    702		cmd.buffer.registers[8].value = 0xFC;
    703
    704		cmd.direction = TRANSFER_WRITE;
    705		cmd.reg_count = 9;
    706
    707		cpia2_send_command(cam, &cmd);
    708	}
    709
    710	/* Activate all settings and start the data stream */
    711	/* Set user mode */
    712	set_default_user_mode(cam);
    713
    714	/* Give VP time to wake up */
    715	schedule_timeout_interruptible(msecs_to_jiffies(100));
    716
    717	set_all_properties(cam);
    718
    719	cpia2_do_command(cam, CPIA2_CMD_GET_USER_MODE, TRANSFER_READ, 0);
    720	DBG("After SetAllProperties(cam), user mode is 0x%0X\n",
    721	    cam->params.vp_params.video_mode);
    722
    723	/***
    724	 * Set audio regulator off.  This and the code to set the compresison
    725	 * state are too complex to form a CPIA2_CMD_, and seem to be somewhat
    726	 * intertwined.  This stuff came straight from the windows driver.
    727	 ***/
    728	/* Turn AutoExposure off in VP and enable the serial bridge to the sensor */
    729	cpia2_do_command(cam, CPIA2_CMD_GET_VP_SYSTEM_CTRL, TRANSFER_READ, 0);
    730	tmp_reg = cam->params.vp_params.system_ctrl;
    731	cmd.buffer.registers[0].value = tmp_reg &
    732		(tmp_reg & (CPIA2_VP_SYSTEMCTRL_HK_CONTROL ^ 0xFF));
    733
    734	cpia2_do_command(cam, CPIA2_CMD_GET_DEVICE_CONFIG, TRANSFER_READ, 0);
    735	cmd.buffer.registers[1].value = cam->params.vp_params.device_config |
    736					CPIA2_VP_DEVICE_CONFIG_SERIAL_BRIDGE;
    737	cmd.buffer.registers[0].index = CPIA2_VP_SYSTEMCTRL;
    738	cmd.buffer.registers[1].index = CPIA2_VP_DEVICE_CONFIG;
    739	cmd.req_mode = CAMERAACCESS_TYPE_RANDOM | CAMERAACCESS_VP;
    740	cmd.reg_count = 2;
    741	cmd.direction = TRANSFER_WRITE;
    742	cmd.start = 0;
    743	cpia2_send_command(cam, &cmd);
    744
    745	/* Set the correct I2C address in the CPiA-2 system register */
    746	cpia2_do_command(cam,
    747			 CPIA2_CMD_SET_SERIAL_ADDR,
    748			 TRANSFER_WRITE,
    749			 CPIA2_SYSTEM_VP_SERIAL_ADDR_SENSOR);
    750
    751	/* Now have sensor access - set bit to turn the audio regulator off */
    752	cpia2_do_command(cam,
    753			 CPIA2_CMD_SET_SENSOR_CR1,
    754			 TRANSFER_WRITE, CPIA2_SENSOR_CR1_DOWN_AUDIO_REGULATOR);
    755
    756	/* Set the correct I2C address in the CPiA-2 system register */
    757	if (cam->params.pnp_id.device_type == DEVICE_STV_672)
    758		cpia2_do_command(cam,
    759				 CPIA2_CMD_SET_SERIAL_ADDR,
    760				 TRANSFER_WRITE,
    761				 CPIA2_SYSTEM_VP_SERIAL_ADDR_VP); // 0x88
    762	else
    763		cpia2_do_command(cam,
    764				 CPIA2_CMD_SET_SERIAL_ADDR,
    765				 TRANSFER_WRITE,
    766				 CPIA2_SYSTEM_VP_SERIAL_ADDR_676_VP); // 0x8a
    767
    768	/* increase signal drive strength */
    769	if (cam->params.pnp_id.device_type == DEVICE_STV_676)
    770		cpia2_do_command(cam,
    771				 CPIA2_CMD_SET_VP_EXP_MODES,
    772				 TRANSFER_WRITE,
    773				 CPIA2_VP_EXPOSURE_MODES_COMPILE_EXP);
    774
    775	/* Start autoexposure */
    776	cpia2_do_command(cam, CPIA2_CMD_GET_DEVICE_CONFIG, TRANSFER_READ, 0);
    777	cmd.buffer.registers[0].value = cam->params.vp_params.device_config &
    778				  (CPIA2_VP_DEVICE_CONFIG_SERIAL_BRIDGE ^ 0xFF);
    779
    780	cpia2_do_command(cam, CPIA2_CMD_GET_VP_SYSTEM_CTRL, TRANSFER_READ, 0);
    781	cmd.buffer.registers[1].value =
    782	    cam->params.vp_params.system_ctrl | CPIA2_VP_SYSTEMCTRL_HK_CONTROL;
    783
    784	cmd.buffer.registers[0].index = CPIA2_VP_DEVICE_CONFIG;
    785	cmd.buffer.registers[1].index = CPIA2_VP_SYSTEMCTRL;
    786	cmd.req_mode = CAMERAACCESS_TYPE_RANDOM | CAMERAACCESS_VP;
    787	cmd.reg_count = 2;
    788	cmd.direction = TRANSFER_WRITE;
    789
    790	cpia2_send_command(cam, &cmd);
    791
    792	/* Set compression state */
    793	cpia2_do_command(cam, CPIA2_CMD_GET_VC_CONTROL, TRANSFER_READ, 0);
    794	if (cam->params.compression.inhibit_htables) {
    795		tmp_reg = cam->params.vc_params.vc_control |
    796			  CPIA2_VC_VC_CTRL_INHIBIT_H_TABLES;
    797	} else  {
    798		tmp_reg = cam->params.vc_params.vc_control &
    799			  ~CPIA2_VC_VC_CTRL_INHIBIT_H_TABLES;
    800	}
    801	cpia2_do_command(cam, CPIA2_CMD_SET_VC_CONTROL, TRANSFER_WRITE,tmp_reg);
    802
    803	/* Set target size (kb) on vc
    804	   This is a heuristic based on the quality parameter and the raw
    805	   framesize in kB divided by 16 (the compression factor when the
    806	   quality is 100%) */
    807	target_kb = (cam->width * cam->height * 2 / 16384) *
    808				cam->params.vc_params.quality / 100;
    809	if (target_kb < 1)
    810		target_kb = 1;
    811	cpia2_do_command(cam, CPIA2_CMD_SET_TARGET_KB,
    812			 TRANSFER_WRITE, target_kb);
    813
    814	/* Wiggle VC Reset */
    815	/***
    816	 * First read and wait a bit.
    817	 ***/
    818	for (i = 0; i < 50; i++) {
    819		cpia2_do_command(cam, CPIA2_CMD_GET_PW_CONTROL,
    820				 TRANSFER_READ, 0);
    821	}
    822
    823	tmp_reg = cam->params.vc_params.pw_control;
    824	tmp_reg &= ~CPIA2_VC_PW_CTRL_VC_RESET_N;
    825
    826	cpia2_do_command(cam, CPIA2_CMD_SET_PW_CONTROL, TRANSFER_WRITE,tmp_reg);
    827
    828	tmp_reg |= CPIA2_VC_PW_CTRL_VC_RESET_N;
    829	cpia2_do_command(cam, CPIA2_CMD_SET_PW_CONTROL, TRANSFER_WRITE,tmp_reg);
    830
    831	cpia2_do_command(cam, CPIA2_CMD_SET_DEF_JPEG_OPT, TRANSFER_WRITE, 0);
    832
    833	cpia2_do_command(cam, CPIA2_CMD_GET_USER_MODE, TRANSFER_READ, 0);
    834	DBG("After VC RESET, user mode is 0x%0X\n",
    835	    cam->params.vp_params.video_mode);
    836
    837	return retval;
    838}
    839
    840/******************************************************************************
    841 *
    842 *  cpia2_set_high_power
    843 *
    844 *****************************************************************************/
    845static int cpia2_set_high_power(struct camera_data *cam)
    846{
    847	int i;
    848	for (i = 0; i <= 50; i++) {
    849		/* Read system status */
    850		cpia2_do_command(cam,CPIA2_CMD_GET_SYSTEM_CTRL,TRANSFER_READ,0);
    851
    852		/* If there is an error, clear it */
    853		if(cam->params.camera_state.system_ctrl &
    854		   CPIA2_SYSTEM_CONTROL_V2W_ERR)
    855			cpia2_do_command(cam, CPIA2_CMD_CLEAR_V2W_ERR,
    856					 TRANSFER_WRITE, 0);
    857
    858		/* Try to set high power mode */
    859		cpia2_do_command(cam, CPIA2_CMD_SET_SYSTEM_CTRL,
    860				 TRANSFER_WRITE, 1);
    861
    862		/* Try to read something in VP to check if everything is awake */
    863		cpia2_do_command(cam, CPIA2_CMD_GET_VP_SYSTEM_STATE,
    864				 TRANSFER_READ, 0);
    865		if (cam->params.vp_params.system_state &
    866		    CPIA2_VP_SYSTEMSTATE_HK_ALIVE) {
    867			break;
    868		} else if (i == 50) {
    869			cam->params.camera_state.power_mode = LO_POWER_MODE;
    870			ERR("Camera did not wake up\n");
    871			return -EIO;
    872		}
    873	}
    874
    875	DBG("System now in high power state\n");
    876	cam->params.camera_state.power_mode = HI_POWER_MODE;
    877	return 0;
    878}
    879
    880/******************************************************************************
    881 *
    882 *  cpia2_set_low_power
    883 *
    884 *****************************************************************************/
    885int cpia2_set_low_power(struct camera_data *cam)
    886{
    887	cam->params.camera_state.power_mode = LO_POWER_MODE;
    888	cpia2_do_command(cam, CPIA2_CMD_SET_SYSTEM_CTRL, TRANSFER_WRITE, 0);
    889	return 0;
    890}
    891
    892/******************************************************************************
    893 *
    894 *  apply_vp_patch
    895 *
    896 *****************************************************************************/
    897static int cpia2_send_onebyte_command(struct camera_data *cam,
    898				      struct cpia2_command *cmd,
    899				      u8 start, u8 datum)
    900{
    901	cmd->buffer.block_data[0] = datum;
    902	cmd->start = start;
    903	cmd->reg_count = 1;
    904	return cpia2_send_command(cam, cmd);
    905}
    906
    907static int apply_vp_patch(struct camera_data *cam)
    908{
    909	const struct firmware *fw;
    910	const char fw_name[] = FIRMWARE;
    911	int i, ret;
    912	struct cpia2_command cmd;
    913
    914	ret = request_firmware(&fw, fw_name, &cam->dev->dev);
    915	if (ret) {
    916		printk(KERN_ERR "cpia2: failed to load VP patch \"%s\"\n",
    917		       fw_name);
    918		return ret;
    919	}
    920
    921	cmd.req_mode = CAMERAACCESS_TYPE_REPEAT | CAMERAACCESS_VP;
    922	cmd.direction = TRANSFER_WRITE;
    923
    924	/* First send the start address... */
    925	cpia2_send_onebyte_command(cam, &cmd, 0x0A, fw->data[0]); /* hi */
    926	cpia2_send_onebyte_command(cam, &cmd, 0x0B, fw->data[1]); /* lo */
    927
    928	/* ... followed by the data payload */
    929	for (i = 2; i < fw->size; i += 64) {
    930		cmd.start = 0x0C; /* Data */
    931		cmd.reg_count = min_t(uint, 64, fw->size - i);
    932		memcpy(cmd.buffer.block_data, &fw->data[i], cmd.reg_count);
    933		cpia2_send_command(cam, &cmd);
    934	}
    935
    936	/* Next send the start address... */
    937	cpia2_send_onebyte_command(cam, &cmd, 0x0A, fw->data[0]); /* hi */
    938	cpia2_send_onebyte_command(cam, &cmd, 0x0B, fw->data[1]); /* lo */
    939
    940	/* ... followed by the 'goto' command */
    941	cpia2_send_onebyte_command(cam, &cmd, 0x0D, 1);
    942
    943	release_firmware(fw);
    944	return 0;
    945}
    946
    947/******************************************************************************
    948 *
    949 *  set_default_user_mode
    950 *
    951 *****************************************************************************/
    952static int set_default_user_mode(struct camera_data *cam)
    953{
    954	unsigned char user_mode;
    955	unsigned char frame_rate;
    956	int width = cam->params.roi.width;
    957	int height = cam->params.roi.height;
    958
    959	switch (cam->params.version.sensor_flags) {
    960	case CPIA2_VP_SENSOR_FLAGS_404:
    961	case CPIA2_VP_SENSOR_FLAGS_407:
    962	case CPIA2_VP_SENSOR_FLAGS_409:
    963	case CPIA2_VP_SENSOR_FLAGS_410:
    964		if ((width > STV_IMAGE_QCIF_COLS)
    965		    || (height > STV_IMAGE_QCIF_ROWS)) {
    966			user_mode = CPIA2_VP_USER_MODE_CIF;
    967		} else {
    968			user_mode = CPIA2_VP_USER_MODE_QCIFDS;
    969		}
    970		frame_rate = CPIA2_VP_FRAMERATE_30;
    971		break;
    972	case CPIA2_VP_SENSOR_FLAGS_500:
    973		if ((width > STV_IMAGE_CIF_COLS)
    974		    || (height > STV_IMAGE_CIF_ROWS)) {
    975			user_mode = CPIA2_VP_USER_MODE_VGA;
    976		} else {
    977			user_mode = CPIA2_VP_USER_MODE_QVGADS;
    978		}
    979		if (cam->params.pnp_id.device_type == DEVICE_STV_672)
    980			frame_rate = CPIA2_VP_FRAMERATE_15;
    981		else
    982			frame_rate = CPIA2_VP_FRAMERATE_30;
    983		break;
    984	default:
    985		LOG("%s: Invalid sensor flag value 0x%0X\n",__func__,
    986		    cam->params.version.sensor_flags);
    987		return -EINVAL;
    988	}
    989
    990	DBG("Sensor flag = 0x%0x, user mode = 0x%0x, frame rate = 0x%X\n",
    991	    cam->params.version.sensor_flags, user_mode, frame_rate);
    992	cpia2_do_command(cam, CPIA2_CMD_SET_USER_MODE, TRANSFER_WRITE,
    993			 user_mode);
    994	if(cam->params.vp_params.frame_rate > 0 &&
    995	   frame_rate > cam->params.vp_params.frame_rate)
    996		frame_rate = cam->params.vp_params.frame_rate;
    997
    998	cpia2_set_fps(cam, frame_rate);
    999
   1000//	if (cam->params.pnp_id.device_type == DEVICE_STV_676)
   1001//		cpia2_do_command(cam,
   1002//				 CPIA2_CMD_SET_VP_SYSTEM_CTRL,
   1003//				 TRANSFER_WRITE,
   1004//				 CPIA2_VP_SYSTEMCTRL_HK_CONTROL |
   1005//				 CPIA2_VP_SYSTEMCTRL_POWER_CONTROL);
   1006
   1007	return 0;
   1008}
   1009
   1010/******************************************************************************
   1011 *
   1012 *  cpia2_match_video_size
   1013 *
   1014 *  return the best match, where 'best' is as always
   1015 *  the largest that is not bigger than what is requested.
   1016 *****************************************************************************/
   1017int cpia2_match_video_size(int width, int height)
   1018{
   1019	if (width >= STV_IMAGE_VGA_COLS && height >= STV_IMAGE_VGA_ROWS)
   1020		return VIDEOSIZE_VGA;
   1021
   1022	if (width >= STV_IMAGE_CIF_COLS && height >= STV_IMAGE_CIF_ROWS)
   1023		return VIDEOSIZE_CIF;
   1024
   1025	if (width >= STV_IMAGE_QVGA_COLS && height >= STV_IMAGE_QVGA_ROWS)
   1026		return VIDEOSIZE_QVGA;
   1027
   1028	if (width >= 288 && height >= 216)
   1029		return VIDEOSIZE_288_216;
   1030
   1031	if (width >= 256 && height >= 192)
   1032		return VIDEOSIZE_256_192;
   1033
   1034	if (width >= 224 && height >= 168)
   1035		return VIDEOSIZE_224_168;
   1036
   1037	if (width >= 192 && height >= 144)
   1038		return VIDEOSIZE_192_144;
   1039
   1040	if (width >= STV_IMAGE_QCIF_COLS && height >= STV_IMAGE_QCIF_ROWS)
   1041		return VIDEOSIZE_QCIF;
   1042
   1043	return -1;
   1044}
   1045
   1046/******************************************************************************
   1047 *
   1048 *  SetVideoSize
   1049 *
   1050 *****************************************************************************/
   1051static int set_vw_size(struct camera_data *cam, int size)
   1052{
   1053	int retval = 0;
   1054
   1055	cam->params.vp_params.video_size = size;
   1056
   1057	switch (size) {
   1058	case VIDEOSIZE_VGA:
   1059		DBG("Setting size to VGA\n");
   1060		cam->params.roi.width = STV_IMAGE_VGA_COLS;
   1061		cam->params.roi.height = STV_IMAGE_VGA_ROWS;
   1062		cam->width = STV_IMAGE_VGA_COLS;
   1063		cam->height = STV_IMAGE_VGA_ROWS;
   1064		break;
   1065	case VIDEOSIZE_CIF:
   1066		DBG("Setting size to CIF\n");
   1067		cam->params.roi.width = STV_IMAGE_CIF_COLS;
   1068		cam->params.roi.height = STV_IMAGE_CIF_ROWS;
   1069		cam->width = STV_IMAGE_CIF_COLS;
   1070		cam->height = STV_IMAGE_CIF_ROWS;
   1071		break;
   1072	case VIDEOSIZE_QVGA:
   1073		DBG("Setting size to QVGA\n");
   1074		cam->params.roi.width = STV_IMAGE_QVGA_COLS;
   1075		cam->params.roi.height = STV_IMAGE_QVGA_ROWS;
   1076		cam->width = STV_IMAGE_QVGA_COLS;
   1077		cam->height = STV_IMAGE_QVGA_ROWS;
   1078		break;
   1079	case VIDEOSIZE_288_216:
   1080		cam->params.roi.width = 288;
   1081		cam->params.roi.height = 216;
   1082		cam->width = 288;
   1083		cam->height = 216;
   1084		break;
   1085	case VIDEOSIZE_256_192:
   1086		cam->width = 256;
   1087		cam->height = 192;
   1088		cam->params.roi.width = 256;
   1089		cam->params.roi.height = 192;
   1090		break;
   1091	case VIDEOSIZE_224_168:
   1092		cam->width = 224;
   1093		cam->height = 168;
   1094		cam->params.roi.width = 224;
   1095		cam->params.roi.height = 168;
   1096		break;
   1097	case VIDEOSIZE_192_144:
   1098		cam->width = 192;
   1099		cam->height = 144;
   1100		cam->params.roi.width = 192;
   1101		cam->params.roi.height = 144;
   1102		break;
   1103	case VIDEOSIZE_QCIF:
   1104		DBG("Setting size to QCIF\n");
   1105		cam->params.roi.width = STV_IMAGE_QCIF_COLS;
   1106		cam->params.roi.height = STV_IMAGE_QCIF_ROWS;
   1107		cam->width = STV_IMAGE_QCIF_COLS;
   1108		cam->height = STV_IMAGE_QCIF_ROWS;
   1109		break;
   1110	default:
   1111		retval = -EINVAL;
   1112	}
   1113	return retval;
   1114}
   1115
   1116/******************************************************************************
   1117 *
   1118 *  configure_sensor
   1119 *
   1120 *****************************************************************************/
   1121static int configure_sensor(struct camera_data *cam,
   1122			    int req_width, int req_height)
   1123{
   1124	int retval;
   1125
   1126	switch (cam->params.version.sensor_flags) {
   1127	case CPIA2_VP_SENSOR_FLAGS_404:
   1128	case CPIA2_VP_SENSOR_FLAGS_407:
   1129	case CPIA2_VP_SENSOR_FLAGS_409:
   1130	case CPIA2_VP_SENSOR_FLAGS_410:
   1131		retval = config_sensor_410(cam, req_width, req_height);
   1132		break;
   1133	case CPIA2_VP_SENSOR_FLAGS_500:
   1134		retval = config_sensor_500(cam, req_width, req_height);
   1135		break;
   1136	default:
   1137		return -EINVAL;
   1138	}
   1139
   1140	return retval;
   1141}
   1142
   1143/******************************************************************************
   1144 *
   1145 *  config_sensor_410
   1146 *
   1147 *****************************************************************************/
   1148static int config_sensor_410(struct camera_data *cam,
   1149			    int req_width, int req_height)
   1150{
   1151	struct cpia2_command cmd;
   1152	int i = 0;
   1153	int image_size;
   1154	int image_type;
   1155	int width = req_width;
   1156	int height = req_height;
   1157
   1158	/***
   1159	 *  Make sure size doesn't exceed CIF.
   1160	 ***/
   1161	if (width > STV_IMAGE_CIF_COLS)
   1162		width = STV_IMAGE_CIF_COLS;
   1163	if (height > STV_IMAGE_CIF_ROWS)
   1164		height = STV_IMAGE_CIF_ROWS;
   1165
   1166	image_size = cpia2_match_video_size(width, height);
   1167
   1168	DBG("Config 410: width = %d, height = %d\n", width, height);
   1169	DBG("Image size returned is %d\n", image_size);
   1170	if (image_size >= 0) {
   1171		set_vw_size(cam, image_size);
   1172		width = cam->params.roi.width;
   1173		height = cam->params.roi.height;
   1174
   1175		DBG("After set_vw_size(), width = %d, height = %d\n",
   1176		    width, height);
   1177		if (width <= 176 && height <= 144) {
   1178			DBG("image type = VIDEOSIZE_QCIF\n");
   1179			image_type = VIDEOSIZE_QCIF;
   1180		}
   1181		else if (width <= 320 && height <= 240) {
   1182			DBG("image type = VIDEOSIZE_QVGA\n");
   1183			image_type = VIDEOSIZE_QVGA;
   1184		}
   1185		else {
   1186			DBG("image type = VIDEOSIZE_CIF\n");
   1187			image_type = VIDEOSIZE_CIF;
   1188		}
   1189	} else {
   1190		ERR("ConfigSensor410 failed\n");
   1191		return -EINVAL;
   1192	}
   1193
   1194	cmd.req_mode = CAMERAACCESS_TYPE_RANDOM | CAMERAACCESS_VC;
   1195	cmd.direction = TRANSFER_WRITE;
   1196
   1197	/* VC Format */
   1198	cmd.buffer.registers[i].index = CPIA2_VC_VC_FORMAT;
   1199	if (image_type == VIDEOSIZE_CIF) {
   1200		cmd.buffer.registers[i++].value =
   1201		    (u8) (CPIA2_VC_VC_FORMAT_UFIRST |
   1202			  CPIA2_VC_VC_FORMAT_SHORTLINE);
   1203	} else {
   1204		cmd.buffer.registers[i++].value =
   1205		    (u8) CPIA2_VC_VC_FORMAT_UFIRST;
   1206	}
   1207
   1208	/* VC Clocks */
   1209	cmd.buffer.registers[i].index = CPIA2_VC_VC_CLOCKS;
   1210	if (image_type == VIDEOSIZE_QCIF) {
   1211		if (cam->params.pnp_id.device_type == DEVICE_STV_672) {
   1212			cmd.buffer.registers[i++].value=
   1213				(u8)(CPIA2_VC_VC_672_CLOCKS_CIF_DIV_BY_3 |
   1214				     CPIA2_VC_VC_672_CLOCKS_SCALING |
   1215				     CPIA2_VC_VC_CLOCKS_LOGDIV2);
   1216			DBG("VC_Clocks (0xc4) should be B\n");
   1217		}
   1218		else {
   1219			cmd.buffer.registers[i++].value=
   1220				(u8)(CPIA2_VC_VC_676_CLOCKS_CIF_DIV_BY_3 |
   1221				     CPIA2_VC_VC_CLOCKS_LOGDIV2);
   1222		}
   1223	} else {
   1224		if (cam->params.pnp_id.device_type == DEVICE_STV_672) {
   1225			cmd.buffer.registers[i++].value =
   1226			   (u8) (CPIA2_VC_VC_672_CLOCKS_CIF_DIV_BY_3 |
   1227				 CPIA2_VC_VC_CLOCKS_LOGDIV0);
   1228		}
   1229		else {
   1230			cmd.buffer.registers[i++].value =
   1231			   (u8) (CPIA2_VC_VC_676_CLOCKS_CIF_DIV_BY_3 |
   1232				 CPIA2_VC_VC_676_CLOCKS_SCALING |
   1233				 CPIA2_VC_VC_CLOCKS_LOGDIV0);
   1234		}
   1235	}
   1236	DBG("VC_Clocks (0xc4) = 0x%0X\n", cmd.buffer.registers[i-1].value);
   1237
   1238	/* Input reqWidth from VC */
   1239	cmd.buffer.registers[i].index = CPIA2_VC_VC_IHSIZE_LO;
   1240	if (image_type == VIDEOSIZE_QCIF)
   1241		cmd.buffer.registers[i++].value =
   1242		    (u8) (STV_IMAGE_QCIF_COLS / 4);
   1243	else
   1244		cmd.buffer.registers[i++].value =
   1245		    (u8) (STV_IMAGE_CIF_COLS / 4);
   1246
   1247	/* Timings */
   1248	cmd.buffer.registers[i].index = CPIA2_VC_VC_XLIM_HI;
   1249	if (image_type == VIDEOSIZE_QCIF)
   1250		cmd.buffer.registers[i++].value = (u8) 0;
   1251	else
   1252		cmd.buffer.registers[i++].value = (u8) 1;
   1253
   1254	cmd.buffer.registers[i].index = CPIA2_VC_VC_XLIM_LO;
   1255	if (image_type == VIDEOSIZE_QCIF)
   1256		cmd.buffer.registers[i++].value = (u8) 208;
   1257	else
   1258		cmd.buffer.registers[i++].value = (u8) 160;
   1259
   1260	cmd.buffer.registers[i].index = CPIA2_VC_VC_YLIM_HI;
   1261	if (image_type == VIDEOSIZE_QCIF)
   1262		cmd.buffer.registers[i++].value = (u8) 0;
   1263	else
   1264		cmd.buffer.registers[i++].value = (u8) 1;
   1265
   1266	cmd.buffer.registers[i].index = CPIA2_VC_VC_YLIM_LO;
   1267	if (image_type == VIDEOSIZE_QCIF)
   1268		cmd.buffer.registers[i++].value = (u8) 160;
   1269	else
   1270		cmd.buffer.registers[i++].value = (u8) 64;
   1271
   1272	/* Output Image Size */
   1273	cmd.buffer.registers[i].index = CPIA2_VC_VC_OHSIZE;
   1274	cmd.buffer.registers[i++].value = cam->params.roi.width / 4;
   1275
   1276	cmd.buffer.registers[i].index = CPIA2_VC_VC_OVSIZE;
   1277	cmd.buffer.registers[i++].value = cam->params.roi.height / 4;
   1278
   1279	/* Cropping */
   1280	cmd.buffer.registers[i].index = CPIA2_VC_VC_HCROP;
   1281	if (image_type == VIDEOSIZE_QCIF)
   1282		cmd.buffer.registers[i++].value =
   1283		    (u8) (((STV_IMAGE_QCIF_COLS / 4) - (width / 4)) / 2);
   1284	else
   1285		cmd.buffer.registers[i++].value =
   1286		    (u8) (((STV_IMAGE_CIF_COLS / 4) - (width / 4)) / 2);
   1287
   1288	cmd.buffer.registers[i].index = CPIA2_VC_VC_VCROP;
   1289	if (image_type == VIDEOSIZE_QCIF)
   1290		cmd.buffer.registers[i++].value =
   1291		    (u8) (((STV_IMAGE_QCIF_ROWS / 4) - (height / 4)) / 2);
   1292	else
   1293		cmd.buffer.registers[i++].value =
   1294		    (u8) (((STV_IMAGE_CIF_ROWS / 4) - (height / 4)) / 2);
   1295
   1296	/* Scaling registers (defaults) */
   1297	cmd.buffer.registers[i].index = CPIA2_VC_VC_HPHASE;
   1298	cmd.buffer.registers[i++].value = (u8) 0;
   1299
   1300	cmd.buffer.registers[i].index = CPIA2_VC_VC_VPHASE;
   1301	cmd.buffer.registers[i++].value = (u8) 0;
   1302
   1303	cmd.buffer.registers[i].index = CPIA2_VC_VC_HISPAN;
   1304	cmd.buffer.registers[i++].value = (u8) 31;
   1305
   1306	cmd.buffer.registers[i].index = CPIA2_VC_VC_VISPAN;
   1307	cmd.buffer.registers[i++].value = (u8) 31;
   1308
   1309	cmd.buffer.registers[i].index = CPIA2_VC_VC_HICROP;
   1310	cmd.buffer.registers[i++].value = (u8) 0;
   1311
   1312	cmd.buffer.registers[i].index = CPIA2_VC_VC_VICROP;
   1313	cmd.buffer.registers[i++].value = (u8) 0;
   1314
   1315	cmd.buffer.registers[i].index = CPIA2_VC_VC_HFRACT;
   1316	cmd.buffer.registers[i++].value = (u8) 0x81;	/* = 8/1 = 8 (HIBYTE/LOBYTE) */
   1317
   1318	cmd.buffer.registers[i].index = CPIA2_VC_VC_VFRACT;
   1319	cmd.buffer.registers[i++].value = (u8) 0x81;	/* = 8/1 = 8 (HIBYTE/LOBYTE) */
   1320
   1321	cmd.reg_count = i;
   1322
   1323	cpia2_send_command(cam, &cmd);
   1324
   1325	return i;
   1326}
   1327
   1328
   1329/******************************************************************************
   1330 *
   1331 *  config_sensor_500(cam)
   1332 *
   1333 *****************************************************************************/
   1334static int config_sensor_500(struct camera_data *cam,
   1335			     int req_width, int req_height)
   1336{
   1337	struct cpia2_command cmd;
   1338	int i = 0;
   1339	int image_size = VIDEOSIZE_CIF;
   1340	int image_type = VIDEOSIZE_VGA;
   1341	int width = req_width;
   1342	int height = req_height;
   1343	unsigned int device = cam->params.pnp_id.device_type;
   1344
   1345	image_size = cpia2_match_video_size(width, height);
   1346
   1347	if (width > STV_IMAGE_CIF_COLS || height > STV_IMAGE_CIF_ROWS)
   1348		image_type = VIDEOSIZE_VGA;
   1349	else if (width > STV_IMAGE_QVGA_COLS || height > STV_IMAGE_QVGA_ROWS)
   1350		image_type = VIDEOSIZE_CIF;
   1351	else if (width > STV_IMAGE_QCIF_COLS || height > STV_IMAGE_QCIF_ROWS)
   1352		image_type = VIDEOSIZE_QVGA;
   1353	else
   1354		image_type = VIDEOSIZE_QCIF;
   1355
   1356	if (image_size >= 0) {
   1357		set_vw_size(cam, image_size);
   1358		width = cam->params.roi.width;
   1359		height = cam->params.roi.height;
   1360	} else {
   1361		ERR("ConfigSensor500 failed\n");
   1362		return -EINVAL;
   1363	}
   1364
   1365	DBG("image_size = %d, width = %d, height = %d, type = %d\n",
   1366	    image_size, width, height, image_type);
   1367
   1368	cmd.req_mode = CAMERAACCESS_TYPE_RANDOM | CAMERAACCESS_VC;
   1369	cmd.direction = TRANSFER_WRITE;
   1370	i = 0;
   1371
   1372	/* VC Format */
   1373	cmd.buffer.registers[i].index = CPIA2_VC_VC_FORMAT;
   1374	cmd.buffer.registers[i].value = (u8) CPIA2_VC_VC_FORMAT_UFIRST;
   1375	if (image_type == VIDEOSIZE_QCIF)
   1376		cmd.buffer.registers[i].value |= (u8) CPIA2_VC_VC_FORMAT_DECIMATING;
   1377	i++;
   1378
   1379	/* VC Clocks */
   1380	cmd.buffer.registers[i].index = CPIA2_VC_VC_CLOCKS;
   1381	if (device == DEVICE_STV_672) {
   1382		if (image_type == VIDEOSIZE_VGA)
   1383			cmd.buffer.registers[i].value =
   1384				(u8)CPIA2_VC_VC_CLOCKS_LOGDIV1;
   1385		else
   1386			cmd.buffer.registers[i].value =
   1387				(u8)(CPIA2_VC_VC_672_CLOCKS_SCALING |
   1388				     CPIA2_VC_VC_CLOCKS_LOGDIV3);
   1389	} else {
   1390		if (image_type == VIDEOSIZE_VGA)
   1391			cmd.buffer.registers[i].value =
   1392				(u8)CPIA2_VC_VC_CLOCKS_LOGDIV0;
   1393		else
   1394			cmd.buffer.registers[i].value =
   1395				(u8)(CPIA2_VC_VC_676_CLOCKS_SCALING |
   1396				     CPIA2_VC_VC_CLOCKS_LOGDIV2);
   1397	}
   1398	i++;
   1399
   1400	DBG("VC_CLOCKS = 0x%X\n", cmd.buffer.registers[i-1].value);
   1401
   1402	/* Input width from VP */
   1403	cmd.buffer.registers[i].index = CPIA2_VC_VC_IHSIZE_LO;
   1404	if (image_type == VIDEOSIZE_VGA)
   1405		cmd.buffer.registers[i].value =
   1406		    (u8) (STV_IMAGE_VGA_COLS / 4);
   1407	else
   1408		cmd.buffer.registers[i].value =
   1409		    (u8) (STV_IMAGE_QVGA_COLS / 4);
   1410	i++;
   1411	DBG("Input width = %d\n", cmd.buffer.registers[i-1].value);
   1412
   1413	/* Timings */
   1414	cmd.buffer.registers[i].index = CPIA2_VC_VC_XLIM_HI;
   1415	if (image_type == VIDEOSIZE_VGA)
   1416		cmd.buffer.registers[i++].value = (u8) 2;
   1417	else
   1418		cmd.buffer.registers[i++].value = (u8) 1;
   1419
   1420	cmd.buffer.registers[i].index = CPIA2_VC_VC_XLIM_LO;
   1421	if (image_type == VIDEOSIZE_VGA)
   1422		cmd.buffer.registers[i++].value = (u8) 250;
   1423	else if (image_type == VIDEOSIZE_QVGA)
   1424		cmd.buffer.registers[i++].value = (u8) 125;
   1425	else
   1426		cmd.buffer.registers[i++].value = (u8) 160;
   1427
   1428	cmd.buffer.registers[i].index = CPIA2_VC_VC_YLIM_HI;
   1429	if (image_type == VIDEOSIZE_VGA)
   1430		cmd.buffer.registers[i++].value = (u8) 2;
   1431	else
   1432		cmd.buffer.registers[i++].value = (u8) 1;
   1433
   1434	cmd.buffer.registers[i].index = CPIA2_VC_VC_YLIM_LO;
   1435	if (image_type == VIDEOSIZE_VGA)
   1436		cmd.buffer.registers[i++].value = (u8) 12;
   1437	else if (image_type == VIDEOSIZE_QVGA)
   1438		cmd.buffer.registers[i++].value = (u8) 64;
   1439	else
   1440		cmd.buffer.registers[i++].value = (u8) 6;
   1441
   1442	/* Output Image Size */
   1443	cmd.buffer.registers[i].index = CPIA2_VC_VC_OHSIZE;
   1444	if (image_type == VIDEOSIZE_QCIF)
   1445		cmd.buffer.registers[i++].value = STV_IMAGE_CIF_COLS  / 4;
   1446	else
   1447		cmd.buffer.registers[i++].value = width / 4;
   1448
   1449	cmd.buffer.registers[i].index = CPIA2_VC_VC_OVSIZE;
   1450	if (image_type == VIDEOSIZE_QCIF)
   1451		cmd.buffer.registers[i++].value = STV_IMAGE_CIF_ROWS  / 4;
   1452	else
   1453		cmd.buffer.registers[i++].value = height / 4;
   1454
   1455	/* Cropping */
   1456	cmd.buffer.registers[i].index = CPIA2_VC_VC_HCROP;
   1457	if (image_type == VIDEOSIZE_VGA)
   1458		cmd.buffer.registers[i++].value =
   1459		    (u8) (((STV_IMAGE_VGA_COLS / 4) - (width / 4)) / 2);
   1460	else if (image_type == VIDEOSIZE_QVGA)
   1461		cmd.buffer.registers[i++].value =
   1462		    (u8) (((STV_IMAGE_QVGA_COLS / 4) - (width / 4)) / 2);
   1463	else if (image_type == VIDEOSIZE_CIF)
   1464		cmd.buffer.registers[i++].value =
   1465		    (u8) (((STV_IMAGE_CIF_COLS / 4) - (width / 4)) / 2);
   1466	else /*if (image_type == VIDEOSIZE_QCIF)*/
   1467		cmd.buffer.registers[i++].value =
   1468			(u8) (((STV_IMAGE_QCIF_COLS / 4) - (width / 4)) / 2);
   1469
   1470	cmd.buffer.registers[i].index = CPIA2_VC_VC_VCROP;
   1471	if (image_type == VIDEOSIZE_VGA)
   1472		cmd.buffer.registers[i++].value =
   1473		    (u8) (((STV_IMAGE_VGA_ROWS / 4) - (height / 4)) / 2);
   1474	else if (image_type == VIDEOSIZE_QVGA)
   1475		cmd.buffer.registers[i++].value =
   1476		    (u8) (((STV_IMAGE_QVGA_ROWS / 4) - (height / 4)) / 2);
   1477	else if (image_type == VIDEOSIZE_CIF)
   1478		cmd.buffer.registers[i++].value =
   1479		    (u8) (((STV_IMAGE_CIF_ROWS / 4) - (height / 4)) / 2);
   1480	else /*if (image_type == VIDEOSIZE_QCIF)*/
   1481		cmd.buffer.registers[i++].value =
   1482		    (u8) (((STV_IMAGE_QCIF_ROWS / 4) - (height / 4)) / 2);
   1483
   1484	/* Scaling registers (defaults) */
   1485	cmd.buffer.registers[i].index = CPIA2_VC_VC_HPHASE;
   1486	if (image_type == VIDEOSIZE_CIF || image_type == VIDEOSIZE_QCIF)
   1487		cmd.buffer.registers[i++].value = (u8) 36;
   1488	else
   1489		cmd.buffer.registers[i++].value = (u8) 0;
   1490
   1491	cmd.buffer.registers[i].index = CPIA2_VC_VC_VPHASE;
   1492	if (image_type == VIDEOSIZE_CIF || image_type == VIDEOSIZE_QCIF)
   1493		cmd.buffer.registers[i++].value = (u8) 32;
   1494	else
   1495		cmd.buffer.registers[i++].value = (u8) 0;
   1496
   1497	cmd.buffer.registers[i].index = CPIA2_VC_VC_HISPAN;
   1498	if (image_type == VIDEOSIZE_CIF || image_type == VIDEOSIZE_QCIF)
   1499		cmd.buffer.registers[i++].value = (u8) 26;
   1500	else
   1501		cmd.buffer.registers[i++].value = (u8) 31;
   1502
   1503	cmd.buffer.registers[i].index = CPIA2_VC_VC_VISPAN;
   1504	if (image_type == VIDEOSIZE_CIF || image_type == VIDEOSIZE_QCIF)
   1505		cmd.buffer.registers[i++].value = (u8) 21;
   1506	else
   1507		cmd.buffer.registers[i++].value = (u8) 31;
   1508
   1509	cmd.buffer.registers[i].index = CPIA2_VC_VC_HICROP;
   1510	cmd.buffer.registers[i++].value = (u8) 0;
   1511
   1512	cmd.buffer.registers[i].index = CPIA2_VC_VC_VICROP;
   1513	cmd.buffer.registers[i++].value = (u8) 0;
   1514
   1515	cmd.buffer.registers[i].index = CPIA2_VC_VC_HFRACT;
   1516	if (image_type == VIDEOSIZE_CIF || image_type == VIDEOSIZE_QCIF)
   1517		cmd.buffer.registers[i++].value = (u8) 0x2B;	/* 2/11 */
   1518	else
   1519		cmd.buffer.registers[i++].value = (u8) 0x81;	/* 8/1 */
   1520
   1521	cmd.buffer.registers[i].index = CPIA2_VC_VC_VFRACT;
   1522	if (image_type == VIDEOSIZE_CIF || image_type == VIDEOSIZE_QCIF)
   1523		cmd.buffer.registers[i++].value = (u8) 0x13;	/* 1/3 */
   1524	else
   1525		cmd.buffer.registers[i++].value = (u8) 0x81;	/* 8/1 */
   1526
   1527	cmd.reg_count = i;
   1528
   1529	cpia2_send_command(cam, &cmd);
   1530
   1531	return i;
   1532}
   1533
   1534
   1535/******************************************************************************
   1536 *
   1537 *  setallproperties
   1538 *
   1539 *  This sets all user changeable properties to the values in cam->params.
   1540 *****************************************************************************/
   1541static int set_all_properties(struct camera_data *cam)
   1542{
   1543	/**
   1544	 * Don't set target_kb here, it will be set later.
   1545	 * framerate and user_mode were already set (set_default_user_mode).
   1546	 **/
   1547
   1548	cpia2_usb_change_streaming_alternate(cam,
   1549					  cam->params.camera_state.stream_mode);
   1550
   1551	cpia2_do_command(cam,
   1552			 CPIA2_CMD_SET_VC_MP_GPIO_DIRECTION,
   1553			 TRANSFER_WRITE, cam->params.vp_params.gpio_direction);
   1554	cpia2_do_command(cam, CPIA2_CMD_SET_VC_MP_GPIO_DATA, TRANSFER_WRITE,
   1555			 cam->params.vp_params.gpio_data);
   1556
   1557	v4l2_ctrl_handler_setup(&cam->hdl);
   1558
   1559	wake_system(cam);
   1560
   1561	set_lowlight_boost(cam);
   1562
   1563	return 0;
   1564}
   1565
   1566/******************************************************************************
   1567 *
   1568 *  cpia2_save_camera_state
   1569 *
   1570 *****************************************************************************/
   1571void cpia2_save_camera_state(struct camera_data *cam)
   1572{
   1573	cpia2_do_command(cam, CPIA2_CMD_GET_USER_EFFECTS, TRANSFER_READ, 0);
   1574	cpia2_do_command(cam, CPIA2_CMD_GET_VC_MP_GPIO_DIRECTION, TRANSFER_READ,
   1575			 0);
   1576	cpia2_do_command(cam, CPIA2_CMD_GET_VC_MP_GPIO_DATA, TRANSFER_READ, 0);
   1577	/* Don't get framerate or target_kb. Trust the values we already have */
   1578}
   1579
   1580
   1581/******************************************************************************
   1582 *
   1583 *  cpia2_set_flicker_mode
   1584 *
   1585 *****************************************************************************/
   1586int cpia2_set_flicker_mode(struct camera_data *cam, int mode)
   1587{
   1588	unsigned char cam_reg;
   1589	int err = 0;
   1590
   1591	if(cam->params.pnp_id.device_type != DEVICE_STV_672)
   1592		return -EINVAL;
   1593
   1594	/* Set the appropriate bits in FLICKER_MODES, preserving the rest */
   1595	if((err = cpia2_do_command(cam, CPIA2_CMD_GET_FLICKER_MODES,
   1596				   TRANSFER_READ, 0)))
   1597		return err;
   1598	cam_reg = cam->params.flicker_control.cam_register;
   1599
   1600	switch(mode) {
   1601	case NEVER_FLICKER:
   1602		cam_reg |= CPIA2_VP_FLICKER_MODES_NEVER_FLICKER;
   1603		cam_reg &= ~CPIA2_VP_FLICKER_MODES_50HZ;
   1604		break;
   1605	case FLICKER_60:
   1606		cam_reg &= ~CPIA2_VP_FLICKER_MODES_NEVER_FLICKER;
   1607		cam_reg &= ~CPIA2_VP_FLICKER_MODES_50HZ;
   1608		break;
   1609	case FLICKER_50:
   1610		cam_reg &= ~CPIA2_VP_FLICKER_MODES_NEVER_FLICKER;
   1611		cam_reg |= CPIA2_VP_FLICKER_MODES_50HZ;
   1612		break;
   1613	default:
   1614		return -EINVAL;
   1615	}
   1616
   1617	if((err = cpia2_do_command(cam, CPIA2_CMD_SET_FLICKER_MODES,
   1618				   TRANSFER_WRITE, cam_reg)))
   1619		return err;
   1620
   1621	/* Set the appropriate bits in EXP_MODES, preserving the rest */
   1622	if((err = cpia2_do_command(cam, CPIA2_CMD_GET_VP_EXP_MODES,
   1623				   TRANSFER_READ, 0)))
   1624		return err;
   1625	cam_reg = cam->params.vp_params.exposure_modes;
   1626
   1627	if (mode == NEVER_FLICKER) {
   1628		cam_reg |= CPIA2_VP_EXPOSURE_MODES_INHIBIT_FLICKER;
   1629	} else {
   1630		cam_reg &= ~CPIA2_VP_EXPOSURE_MODES_INHIBIT_FLICKER;
   1631	}
   1632
   1633	if((err = cpia2_do_command(cam, CPIA2_CMD_SET_VP_EXP_MODES,
   1634				   TRANSFER_WRITE, cam_reg)))
   1635		return err;
   1636
   1637	if((err = cpia2_do_command(cam, CPIA2_CMD_REHASH_VP4,
   1638				   TRANSFER_WRITE, 1)))
   1639		return err;
   1640
   1641	switch(mode) {
   1642	case NEVER_FLICKER:
   1643	case FLICKER_60:
   1644	case FLICKER_50:
   1645		cam->params.flicker_control.flicker_mode_req = mode;
   1646		break;
   1647	default:
   1648		err = -EINVAL;
   1649	}
   1650
   1651	return err;
   1652}
   1653
   1654/******************************************************************************
   1655 *
   1656 *  cpia2_set_property_flip
   1657 *
   1658 *****************************************************************************/
   1659void cpia2_set_property_flip(struct camera_data *cam, int prop_val)
   1660{
   1661	unsigned char cam_reg;
   1662
   1663	cpia2_do_command(cam, CPIA2_CMD_GET_USER_EFFECTS, TRANSFER_READ, 0);
   1664	cam_reg = cam->params.vp_params.user_effects;
   1665
   1666	if (prop_val)
   1667	{
   1668		cam_reg |= CPIA2_VP_USER_EFFECTS_FLIP;
   1669	}
   1670	else
   1671	{
   1672		cam_reg &= ~CPIA2_VP_USER_EFFECTS_FLIP;
   1673	}
   1674	cam->params.vp_params.user_effects = cam_reg;
   1675	cpia2_do_command(cam, CPIA2_CMD_SET_USER_EFFECTS, TRANSFER_WRITE,
   1676			 cam_reg);
   1677}
   1678
   1679/******************************************************************************
   1680 *
   1681 *  cpia2_set_property_mirror
   1682 *
   1683 *****************************************************************************/
   1684void cpia2_set_property_mirror(struct camera_data *cam, int prop_val)
   1685{
   1686	unsigned char cam_reg;
   1687
   1688	cpia2_do_command(cam, CPIA2_CMD_GET_USER_EFFECTS, TRANSFER_READ, 0);
   1689	cam_reg = cam->params.vp_params.user_effects;
   1690
   1691	if (prop_val)
   1692	{
   1693		cam_reg |= CPIA2_VP_USER_EFFECTS_MIRROR;
   1694	}
   1695	else
   1696	{
   1697		cam_reg &= ~CPIA2_VP_USER_EFFECTS_MIRROR;
   1698	}
   1699	cam->params.vp_params.user_effects = cam_reg;
   1700	cpia2_do_command(cam, CPIA2_CMD_SET_USER_EFFECTS, TRANSFER_WRITE,
   1701			 cam_reg);
   1702}
   1703
   1704/******************************************************************************
   1705 *
   1706 *  cpia2_set_gpio
   1707 *
   1708 *****************************************************************************/
   1709int cpia2_set_gpio(struct camera_data *cam, unsigned char setting)
   1710{
   1711	int ret;
   1712
   1713	/* Set the microport direction (register 0x90, should be defined
   1714	 * already) to 1 (user output), and set the microport data (0x91) to
   1715	 * the value in the ioctl argument.
   1716	 */
   1717
   1718	ret = cpia2_do_command(cam,
   1719			       CPIA2_CMD_SET_VC_MP_GPIO_DIRECTION,
   1720			       CPIA2_VC_MP_DIR_OUTPUT,
   1721			       255);
   1722	if (ret < 0)
   1723		return ret;
   1724	cam->params.vp_params.gpio_direction = 255;
   1725
   1726	ret = cpia2_do_command(cam,
   1727			       CPIA2_CMD_SET_VC_MP_GPIO_DATA,
   1728			       CPIA2_VC_MP_DIR_OUTPUT,
   1729			       setting);
   1730	if (ret < 0)
   1731		return ret;
   1732	cam->params.vp_params.gpio_data = setting;
   1733
   1734	return 0;
   1735}
   1736
   1737/******************************************************************************
   1738 *
   1739 *  cpia2_set_fps
   1740 *
   1741 *****************************************************************************/
   1742int cpia2_set_fps(struct camera_data *cam, int framerate)
   1743{
   1744	int retval;
   1745
   1746	switch(framerate) {
   1747		case CPIA2_VP_FRAMERATE_30:
   1748		case CPIA2_VP_FRAMERATE_25:
   1749			if(cam->params.pnp_id.device_type == DEVICE_STV_672 &&
   1750			   cam->params.version.sensor_flags ==
   1751						    CPIA2_VP_SENSOR_FLAGS_500) {
   1752				return -EINVAL;
   1753			}
   1754			fallthrough;
   1755		case CPIA2_VP_FRAMERATE_15:
   1756		case CPIA2_VP_FRAMERATE_12_5:
   1757		case CPIA2_VP_FRAMERATE_7_5:
   1758		case CPIA2_VP_FRAMERATE_6_25:
   1759			break;
   1760		default:
   1761			return -EINVAL;
   1762	}
   1763
   1764	if (cam->params.pnp_id.device_type == DEVICE_STV_672 &&
   1765	    framerate == CPIA2_VP_FRAMERATE_15)
   1766		framerate = 0; /* Work around bug in VP4 */
   1767
   1768	retval = cpia2_do_command(cam,
   1769				 CPIA2_CMD_FRAMERATE_REQ,
   1770				 TRANSFER_WRITE,
   1771				 framerate);
   1772
   1773	if(retval == 0)
   1774		cam->params.vp_params.frame_rate = framerate;
   1775
   1776	return retval;
   1777}
   1778
   1779/******************************************************************************
   1780 *
   1781 *  cpia2_set_brightness
   1782 *
   1783 *****************************************************************************/
   1784void cpia2_set_brightness(struct camera_data *cam, unsigned char value)
   1785{
   1786	/***
   1787	 * Don't let the register be set to zero - bug in VP4 - flash of full
   1788	 * brightness
   1789	 ***/
   1790	if (cam->params.pnp_id.device_type == DEVICE_STV_672 && value == 0)
   1791		value++;
   1792	DBG("Setting brightness to %d (0x%0x)\n", value, value);
   1793	cpia2_do_command(cam, CPIA2_CMD_SET_VP_BRIGHTNESS, TRANSFER_WRITE, value);
   1794}
   1795
   1796/******************************************************************************
   1797 *
   1798 *  cpia2_set_contrast
   1799 *
   1800 *****************************************************************************/
   1801void cpia2_set_contrast(struct camera_data *cam, unsigned char value)
   1802{
   1803	DBG("Setting contrast to %d (0x%0x)\n", value, value);
   1804	cpia2_do_command(cam, CPIA2_CMD_SET_CONTRAST, TRANSFER_WRITE, value);
   1805}
   1806
   1807/******************************************************************************
   1808 *
   1809 *  cpia2_set_saturation
   1810 *
   1811 *****************************************************************************/
   1812void cpia2_set_saturation(struct camera_data *cam, unsigned char value)
   1813{
   1814	DBG("Setting saturation to %d (0x%0x)\n", value, value);
   1815	cpia2_do_command(cam,CPIA2_CMD_SET_VP_SATURATION, TRANSFER_WRITE,value);
   1816}
   1817
   1818/******************************************************************************
   1819 *
   1820 *  wake_system
   1821 *
   1822 *****************************************************************************/
   1823static void wake_system(struct camera_data *cam)
   1824{
   1825	cpia2_do_command(cam, CPIA2_CMD_SET_WAKEUP, TRANSFER_WRITE, 0);
   1826}
   1827
   1828/******************************************************************************
   1829 *
   1830 *  set_lowlight_boost
   1831 *
   1832 *  Valid for STV500 sensor only
   1833 *****************************************************************************/
   1834static void set_lowlight_boost(struct camera_data *cam)
   1835{
   1836	struct cpia2_command cmd;
   1837
   1838	if (cam->params.pnp_id.device_type != DEVICE_STV_672 ||
   1839	    cam->params.version.sensor_flags != CPIA2_VP_SENSOR_FLAGS_500)
   1840		return;
   1841
   1842	cmd.direction = TRANSFER_WRITE;
   1843	cmd.req_mode = CAMERAACCESS_TYPE_BLOCK | CAMERAACCESS_VP;
   1844	cmd.reg_count = 3;
   1845	cmd.start = CPIA2_VP_RAM_ADDR_H;
   1846
   1847	cmd.buffer.block_data[0] = 0;	/* High byte of address to write to */
   1848	cmd.buffer.block_data[1] = 0x59;	/* Low byte of address to write to */
   1849	cmd.buffer.block_data[2] = 0;	/* High byte of data to write */
   1850
   1851	cpia2_send_command(cam, &cmd);
   1852
   1853	if (cam->params.vp_params.lowlight_boost) {
   1854		cmd.buffer.block_data[0] = 0x02;	/* Low byte data to write */
   1855	} else {
   1856		cmd.buffer.block_data[0] = 0x06;
   1857	}
   1858	cmd.start = CPIA2_VP_RAM_DATA;
   1859	cmd.reg_count = 1;
   1860	cpia2_send_command(cam, &cmd);
   1861
   1862	/* Rehash the VP4 values */
   1863	cpia2_do_command(cam, CPIA2_CMD_REHASH_VP4, TRANSFER_WRITE, 1);
   1864}
   1865
   1866/******************************************************************************
   1867 *
   1868 *  cpia2_set_format
   1869 *
   1870 *  Assumes that new size is already set in param struct.
   1871 *****************************************************************************/
   1872void cpia2_set_format(struct camera_data *cam)
   1873{
   1874	cam->flush = true;
   1875
   1876	cpia2_usb_stream_pause(cam);
   1877
   1878	/* reset camera to new size */
   1879	cpia2_set_low_power(cam);
   1880	cpia2_reset_camera(cam);
   1881	cam->flush = false;
   1882
   1883	cpia2_dbg_dump_registers(cam);
   1884
   1885	cpia2_usb_stream_resume(cam);
   1886}
   1887
   1888/******************************************************************************
   1889 *
   1890 * cpia2_dbg_dump_registers
   1891 *
   1892 *****************************************************************************/
   1893void cpia2_dbg_dump_registers(struct camera_data *cam)
   1894{
   1895#ifdef _CPIA2_DEBUG_
   1896	struct cpia2_command cmd;
   1897
   1898	if (!(debugs_on & DEBUG_DUMP_REGS))
   1899		return;
   1900
   1901	cmd.direction = TRANSFER_READ;
   1902
   1903	/* Start with bank 0 (SYSTEM) */
   1904	cmd.req_mode = CAMERAACCESS_TYPE_BLOCK | CAMERAACCESS_SYSTEM;
   1905	cmd.reg_count = 3;
   1906	cmd.start = 0;
   1907	cpia2_send_command(cam, &cmd);
   1908	printk(KERN_DEBUG "System Device Hi      = 0x%X\n",
   1909	       cmd.buffer.block_data[0]);
   1910	printk(KERN_DEBUG "System Device Lo      = 0x%X\n",
   1911	       cmd.buffer.block_data[1]);
   1912	printk(KERN_DEBUG "System_system control = 0x%X\n",
   1913	       cmd.buffer.block_data[2]);
   1914
   1915	/* Bank 1 (VC) */
   1916	cmd.req_mode = CAMERAACCESS_TYPE_BLOCK | CAMERAACCESS_VC;
   1917	cmd.reg_count = 4;
   1918	cmd.start = 0x80;
   1919	cpia2_send_command(cam, &cmd);
   1920	printk(KERN_DEBUG "ASIC_ID       = 0x%X\n",
   1921	       cmd.buffer.block_data[0]);
   1922	printk(KERN_DEBUG "ASIC_REV      = 0x%X\n",
   1923	       cmd.buffer.block_data[1]);
   1924	printk(KERN_DEBUG "PW_CONTRL     = 0x%X\n",
   1925	       cmd.buffer.block_data[2]);
   1926	printk(KERN_DEBUG "WAKEUP        = 0x%X\n",
   1927	       cmd.buffer.block_data[3]);
   1928
   1929	cmd.start = 0xA0;	/* ST_CTRL */
   1930	cmd.reg_count = 1;
   1931	cpia2_send_command(cam, &cmd);
   1932	printk(KERN_DEBUG "Stream ctrl   = 0x%X\n",
   1933	       cmd.buffer.block_data[0]);
   1934
   1935	cmd.start = 0xA4;	/* Stream status */
   1936	cpia2_send_command(cam, &cmd);
   1937	printk(KERN_DEBUG "Stream status = 0x%X\n",
   1938	       cmd.buffer.block_data[0]);
   1939
   1940	cmd.start = 0xA8;	/* USB status */
   1941	cmd.reg_count = 3;
   1942	cpia2_send_command(cam, &cmd);
   1943	printk(KERN_DEBUG "USB_CTRL      = 0x%X\n",
   1944	       cmd.buffer.block_data[0]);
   1945	printk(KERN_DEBUG "USB_STRM      = 0x%X\n",
   1946	       cmd.buffer.block_data[1]);
   1947	printk(KERN_DEBUG "USB_STATUS    = 0x%X\n",
   1948	       cmd.buffer.block_data[2]);
   1949
   1950	cmd.start = 0xAF;	/* USB settings */
   1951	cmd.reg_count = 1;
   1952	cpia2_send_command(cam, &cmd);
   1953	printk(KERN_DEBUG "USB settings  = 0x%X\n",
   1954	       cmd.buffer.block_data[0]);
   1955
   1956	cmd.start = 0xC0;	/* VC stuff */
   1957	cmd.reg_count = 26;
   1958	cpia2_send_command(cam, &cmd);
   1959	printk(KERN_DEBUG "VC Control    = 0x%0X\n",
   1960	       cmd.buffer.block_data[0]);
   1961	printk(KERN_DEBUG "VC Format     = 0x%0X\n",
   1962	       cmd.buffer.block_data[3]);
   1963	printk(KERN_DEBUG "VC Clocks     = 0x%0X\n",
   1964	       cmd.buffer.block_data[4]);
   1965	printk(KERN_DEBUG "VC IHSize     = 0x%0X\n",
   1966	       cmd.buffer.block_data[5]);
   1967	printk(KERN_DEBUG "VC Xlim Hi    = 0x%0X\n",
   1968	       cmd.buffer.block_data[6]);
   1969	printk(KERN_DEBUG "VC XLim Lo    = 0x%0X\n",
   1970	       cmd.buffer.block_data[7]);
   1971	printk(KERN_DEBUG "VC YLim Hi    = 0x%0X\n",
   1972	       cmd.buffer.block_data[8]);
   1973	printk(KERN_DEBUG "VC YLim Lo    = 0x%0X\n",
   1974	       cmd.buffer.block_data[9]);
   1975	printk(KERN_DEBUG "VC OHSize     = 0x%0X\n",
   1976	       cmd.buffer.block_data[10]);
   1977	printk(KERN_DEBUG "VC OVSize     = 0x%0X\n",
   1978	       cmd.buffer.block_data[11]);
   1979	printk(KERN_DEBUG "VC HCrop      = 0x%0X\n",
   1980	       cmd.buffer.block_data[12]);
   1981	printk(KERN_DEBUG "VC VCrop      = 0x%0X\n",
   1982	       cmd.buffer.block_data[13]);
   1983	printk(KERN_DEBUG "VC HPhase     = 0x%0X\n",
   1984	       cmd.buffer.block_data[14]);
   1985	printk(KERN_DEBUG "VC VPhase     = 0x%0X\n",
   1986	       cmd.buffer.block_data[15]);
   1987	printk(KERN_DEBUG "VC HIspan     = 0x%0X\n",
   1988	       cmd.buffer.block_data[16]);
   1989	printk(KERN_DEBUG "VC VIspan     = 0x%0X\n",
   1990	       cmd.buffer.block_data[17]);
   1991	printk(KERN_DEBUG "VC HiCrop     = 0x%0X\n",
   1992	       cmd.buffer.block_data[18]);
   1993	printk(KERN_DEBUG "VC ViCrop     = 0x%0X\n",
   1994	       cmd.buffer.block_data[19]);
   1995	printk(KERN_DEBUG "VC HiFract    = 0x%0X\n",
   1996	       cmd.buffer.block_data[20]);
   1997	printk(KERN_DEBUG "VC ViFract    = 0x%0X\n",
   1998	       cmd.buffer.block_data[21]);
   1999	printk(KERN_DEBUG "VC JPeg Opt   = 0x%0X\n",
   2000	       cmd.buffer.block_data[22]);
   2001	printk(KERN_DEBUG "VC Creep Per  = 0x%0X\n",
   2002	       cmd.buffer.block_data[23]);
   2003	printk(KERN_DEBUG "VC User Sq.   = 0x%0X\n",
   2004	       cmd.buffer.block_data[24]);
   2005	printk(KERN_DEBUG "VC Target KB  = 0x%0X\n",
   2006	       cmd.buffer.block_data[25]);
   2007
   2008	/*** VP ***/
   2009	cmd.req_mode = CAMERAACCESS_TYPE_BLOCK | CAMERAACCESS_VP;
   2010	cmd.reg_count = 14;
   2011	cmd.start = 0;
   2012	cpia2_send_command(cam, &cmd);
   2013
   2014	printk(KERN_DEBUG "VP Dev Hi     = 0x%0X\n",
   2015	       cmd.buffer.block_data[0]);
   2016	printk(KERN_DEBUG "VP Dev Lo     = 0x%0X\n",
   2017	       cmd.buffer.block_data[1]);
   2018	printk(KERN_DEBUG "VP Sys State  = 0x%0X\n",
   2019	       cmd.buffer.block_data[2]);
   2020	printk(KERN_DEBUG "VP Sys Ctrl   = 0x%0X\n",
   2021	       cmd.buffer.block_data[3]);
   2022	printk(KERN_DEBUG "VP Sensor flg = 0x%0X\n",
   2023	       cmd.buffer.block_data[5]);
   2024	printk(KERN_DEBUG "VP Sensor Rev = 0x%0X\n",
   2025	       cmd.buffer.block_data[6]);
   2026	printk(KERN_DEBUG "VP Dev Config = 0x%0X\n",
   2027	       cmd.buffer.block_data[7]);
   2028	printk(KERN_DEBUG "VP GPIO_DIR   = 0x%0X\n",
   2029	       cmd.buffer.block_data[8]);
   2030	printk(KERN_DEBUG "VP GPIO_DATA  = 0x%0X\n",
   2031	       cmd.buffer.block_data[9]);
   2032	printk(KERN_DEBUG "VP Ram ADDR H = 0x%0X\n",
   2033	       cmd.buffer.block_data[10]);
   2034	printk(KERN_DEBUG "VP Ram ADDR L = 0x%0X\n",
   2035	       cmd.buffer.block_data[11]);
   2036	printk(KERN_DEBUG "VP RAM Data   = 0x%0X\n",
   2037	       cmd.buffer.block_data[12]);
   2038	printk(KERN_DEBUG "Do Call       = 0x%0X\n",
   2039	       cmd.buffer.block_data[13]);
   2040
   2041	if (cam->params.pnp_id.device_type == DEVICE_STV_672) {
   2042		cmd.reg_count = 9;
   2043		cmd.start = 0x0E;
   2044		cpia2_send_command(cam, &cmd);
   2045		printk(KERN_DEBUG "VP Clock Ctrl = 0x%0X\n",
   2046		       cmd.buffer.block_data[0]);
   2047		printk(KERN_DEBUG "VP Patch Rev  = 0x%0X\n",
   2048		       cmd.buffer.block_data[1]);
   2049		printk(KERN_DEBUG "VP Vid Mode   = 0x%0X\n",
   2050		       cmd.buffer.block_data[2]);
   2051		printk(KERN_DEBUG "VP Framerate  = 0x%0X\n",
   2052		       cmd.buffer.block_data[3]);
   2053		printk(KERN_DEBUG "VP UserEffect = 0x%0X\n",
   2054		       cmd.buffer.block_data[4]);
   2055		printk(KERN_DEBUG "VP White Bal  = 0x%0X\n",
   2056		       cmd.buffer.block_data[5]);
   2057		printk(KERN_DEBUG "VP WB thresh  = 0x%0X\n",
   2058		       cmd.buffer.block_data[6]);
   2059		printk(KERN_DEBUG "VP Exp Modes  = 0x%0X\n",
   2060		       cmd.buffer.block_data[7]);
   2061		printk(KERN_DEBUG "VP Exp Target = 0x%0X\n",
   2062		       cmd.buffer.block_data[8]);
   2063
   2064		cmd.reg_count = 1;
   2065		cmd.start = 0x1B;
   2066		cpia2_send_command(cam, &cmd);
   2067		printk(KERN_DEBUG "VP FlickerMds = 0x%0X\n",
   2068		       cmd.buffer.block_data[0]);
   2069	} else {
   2070		cmd.reg_count = 8 ;
   2071		cmd.start = 0x0E;
   2072		cpia2_send_command(cam, &cmd);
   2073		printk(KERN_DEBUG "VP Clock Ctrl = 0x%0X\n",
   2074		       cmd.buffer.block_data[0]);
   2075		printk(KERN_DEBUG "VP Patch Rev  = 0x%0X\n",
   2076		       cmd.buffer.block_data[1]);
   2077		printk(KERN_DEBUG "VP Vid Mode   = 0x%0X\n",
   2078		       cmd.buffer.block_data[5]);
   2079		printk(KERN_DEBUG "VP Framerate  = 0x%0X\n",
   2080		       cmd.buffer.block_data[6]);
   2081		printk(KERN_DEBUG "VP UserEffect = 0x%0X\n",
   2082		       cmd.buffer.block_data[7]);
   2083
   2084		cmd.reg_count = 1;
   2085		cmd.start = CPIA2_VP5_EXPOSURE_TARGET;
   2086		cpia2_send_command(cam, &cmd);
   2087		printk(KERN_DEBUG "VP5 Exp Target= 0x%0X\n",
   2088		       cmd.buffer.block_data[0]);
   2089
   2090		cmd.reg_count = 4;
   2091		cmd.start = 0x3A;
   2092		cpia2_send_command(cam, &cmd);
   2093		printk(KERN_DEBUG "VP5 MY Black  = 0x%0X\n",
   2094		       cmd.buffer.block_data[0]);
   2095		printk(KERN_DEBUG "VP5 MCY Range = 0x%0X\n",
   2096		       cmd.buffer.block_data[1]);
   2097		printk(KERN_DEBUG "VP5 MYCEILING = 0x%0X\n",
   2098		       cmd.buffer.block_data[2]);
   2099		printk(KERN_DEBUG "VP5 MCUV Sat  = 0x%0X\n",
   2100		       cmd.buffer.block_data[3]);
   2101	}
   2102#endif
   2103}
   2104
   2105/******************************************************************************
   2106 *
   2107 *  reset_camera_struct
   2108 *
   2109 *  Sets all values to the defaults
   2110 *****************************************************************************/
   2111static void reset_camera_struct(struct camera_data *cam)
   2112{
   2113	/***
   2114	 * The following parameter values are the defaults from the register map.
   2115	 ***/
   2116	cam->params.vp_params.lowlight_boost = 0;
   2117
   2118	/* FlickerModes */
   2119	cam->params.flicker_control.flicker_mode_req = NEVER_FLICKER;
   2120
   2121	/* jpeg params */
   2122	cam->params.compression.jpeg_options = CPIA2_VC_VC_JPEG_OPT_DEFAULT;
   2123	cam->params.compression.creep_period = 2;
   2124	cam->params.compression.user_squeeze = 20;
   2125	cam->params.compression.inhibit_htables = false;
   2126
   2127	/* gpio params */
   2128	cam->params.vp_params.gpio_direction = 0;	/* write, the default safe mode */
   2129	cam->params.vp_params.gpio_data = 0;
   2130
   2131	/* Target kb params */
   2132	cam->params.vc_params.quality = 100;
   2133
   2134	/***
   2135	 * Set Sensor FPS as fast as possible.
   2136	 ***/
   2137	if(cam->params.pnp_id.device_type == DEVICE_STV_672) {
   2138		if(cam->params.version.sensor_flags == CPIA2_VP_SENSOR_FLAGS_500)
   2139			cam->params.vp_params.frame_rate = CPIA2_VP_FRAMERATE_15;
   2140		else
   2141			cam->params.vp_params.frame_rate = CPIA2_VP_FRAMERATE_30;
   2142	} else {
   2143		cam->params.vp_params.frame_rate = CPIA2_VP_FRAMERATE_30;
   2144	}
   2145
   2146	/***
   2147	 * Set default video mode as large as possible :
   2148	 * for vga sensor set to vga, for cif sensor set to CIF.
   2149	 ***/
   2150	if (cam->params.version.sensor_flags == CPIA2_VP_SENSOR_FLAGS_500) {
   2151		cam->sensor_type = CPIA2_SENSOR_500;
   2152		cam->video_size = VIDEOSIZE_VGA;
   2153		cam->params.roi.width = STV_IMAGE_VGA_COLS;
   2154		cam->params.roi.height = STV_IMAGE_VGA_ROWS;
   2155	} else {
   2156		cam->sensor_type = CPIA2_SENSOR_410;
   2157		cam->video_size = VIDEOSIZE_CIF;
   2158		cam->params.roi.width = STV_IMAGE_CIF_COLS;
   2159		cam->params.roi.height = STV_IMAGE_CIF_ROWS;
   2160	}
   2161
   2162	cam->width = cam->params.roi.width;
   2163	cam->height = cam->params.roi.height;
   2164}
   2165
   2166/******************************************************************************
   2167 *
   2168 *  cpia2_init_camera_struct
   2169 *
   2170 *  Deinitialize camera struct
   2171 *****************************************************************************/
   2172void cpia2_deinit_camera_struct(struct camera_data *cam, struct usb_interface *intf)
   2173{
   2174	v4l2_device_unregister(&cam->v4l2_dev);
   2175	kfree(cam);
   2176}
   2177
   2178/******************************************************************************
   2179 *
   2180 *  cpia2_init_camera_struct
   2181 *
   2182 *  Initializes camera struct, does not call reset to fill in defaults.
   2183 *****************************************************************************/
   2184struct camera_data *cpia2_init_camera_struct(struct usb_interface *intf)
   2185{
   2186	struct camera_data *cam;
   2187
   2188	cam = kzalloc(sizeof(*cam), GFP_KERNEL);
   2189
   2190	if (!cam) {
   2191		ERR("couldn't kmalloc cpia2 struct\n");
   2192		return NULL;
   2193	}
   2194
   2195	cam->v4l2_dev.release = cpia2_camera_release;
   2196	if (v4l2_device_register(&intf->dev, &cam->v4l2_dev) < 0) {
   2197		v4l2_err(&cam->v4l2_dev, "couldn't register v4l2_device\n");
   2198		kfree(cam);
   2199		return NULL;
   2200	}
   2201
   2202	mutex_init(&cam->v4l2_lock);
   2203	init_waitqueue_head(&cam->wq_stream);
   2204
   2205	return cam;
   2206}
   2207
   2208/******************************************************************************
   2209 *
   2210 *  cpia2_init_camera
   2211 *
   2212 *  Initializes camera.
   2213 *****************************************************************************/
   2214int cpia2_init_camera(struct camera_data *cam)
   2215{
   2216	DBG("Start\n");
   2217
   2218	cam->mmapped = false;
   2219
   2220	/* Get sensor and asic types before reset. */
   2221	cpia2_set_high_power(cam);
   2222	cpia2_get_version_info(cam);
   2223	if (cam->params.version.asic_id != CPIA2_ASIC_672) {
   2224		ERR("Device IO error (asicID has incorrect value of 0x%X\n",
   2225		    cam->params.version.asic_id);
   2226		return -ENODEV;
   2227	}
   2228
   2229	/* Set GPIO direction and data to a safe state. */
   2230	cpia2_do_command(cam, CPIA2_CMD_SET_VC_MP_GPIO_DIRECTION,
   2231			 TRANSFER_WRITE, 0);
   2232	cpia2_do_command(cam, CPIA2_CMD_SET_VC_MP_GPIO_DATA,
   2233			 TRANSFER_WRITE, 0);
   2234
   2235	/* resetting struct requires version info for sensor and asic types */
   2236	reset_camera_struct(cam);
   2237
   2238	cpia2_set_low_power(cam);
   2239
   2240	DBG("End\n");
   2241
   2242	return 0;
   2243}
   2244
   2245/******************************************************************************
   2246 *
   2247 *  cpia2_allocate_buffers
   2248 *
   2249 *****************************************************************************/
   2250int cpia2_allocate_buffers(struct camera_data *cam)
   2251{
   2252	int i;
   2253
   2254	if(!cam->buffers) {
   2255		u32 size = cam->num_frames*sizeof(struct framebuf);
   2256		cam->buffers = kmalloc(size, GFP_KERNEL);
   2257		if(!cam->buffers) {
   2258			ERR("couldn't kmalloc frame buffer structures\n");
   2259			return -ENOMEM;
   2260		}
   2261	}
   2262
   2263	if(!cam->frame_buffer) {
   2264		cam->frame_buffer = rvmalloc(cam->frame_size*cam->num_frames);
   2265		if (!cam->frame_buffer) {
   2266			ERR("couldn't vmalloc frame buffer data area\n");
   2267			kfree(cam->buffers);
   2268			cam->buffers = NULL;
   2269			return -ENOMEM;
   2270		}
   2271	}
   2272
   2273	for(i=0; i<cam->num_frames-1; ++i) {
   2274		cam->buffers[i].next = &cam->buffers[i+1];
   2275		cam->buffers[i].data = cam->frame_buffer +i*cam->frame_size;
   2276		cam->buffers[i].status = FRAME_EMPTY;
   2277		cam->buffers[i].length = 0;
   2278		cam->buffers[i].max_length = 0;
   2279		cam->buffers[i].num = i;
   2280	}
   2281	cam->buffers[i].next = cam->buffers;
   2282	cam->buffers[i].data = cam->frame_buffer +i*cam->frame_size;
   2283	cam->buffers[i].status = FRAME_EMPTY;
   2284	cam->buffers[i].length = 0;
   2285	cam->buffers[i].max_length = 0;
   2286	cam->buffers[i].num = i;
   2287	cam->curbuff = cam->buffers;
   2288	cam->workbuff = cam->curbuff->next;
   2289	DBG("buffers=%p, curbuff=%p, workbuff=%p\n", cam->buffers, cam->curbuff,
   2290	    cam->workbuff);
   2291	return 0;
   2292}
   2293
   2294/******************************************************************************
   2295 *
   2296 *  cpia2_free_buffers
   2297 *
   2298 *****************************************************************************/
   2299void cpia2_free_buffers(struct camera_data *cam)
   2300{
   2301	if(cam->buffers) {
   2302		kfree(cam->buffers);
   2303		cam->buffers = NULL;
   2304	}
   2305	if(cam->frame_buffer) {
   2306		rvfree(cam->frame_buffer, cam->frame_size*cam->num_frames);
   2307		cam->frame_buffer = NULL;
   2308	}
   2309}
   2310
   2311/******************************************************************************
   2312 *
   2313 *  cpia2_read
   2314 *
   2315 *****************************************************************************/
   2316long cpia2_read(struct camera_data *cam,
   2317		char __user *buf, unsigned long count, int noblock)
   2318{
   2319	struct framebuf *frame;
   2320
   2321	if (!count)
   2322		return 0;
   2323
   2324	if (!buf) {
   2325		ERR("%s: buffer NULL\n",__func__);
   2326		return -EINVAL;
   2327	}
   2328
   2329	if (!cam) {
   2330		ERR("%s: Internal error, camera_data NULL!\n",__func__);
   2331		return -EINVAL;
   2332	}
   2333
   2334	if (!cam->streaming) {
   2335		/* Start streaming */
   2336		cpia2_usb_stream_start(cam,
   2337				       cam->params.camera_state.stream_mode);
   2338	}
   2339
   2340	/* Copy cam->curbuff in case it changes while we're processing */
   2341	frame = cam->curbuff;
   2342	if (noblock && frame->status != FRAME_READY) {
   2343		return -EAGAIN;
   2344	}
   2345
   2346	if (frame->status != FRAME_READY) {
   2347		mutex_unlock(&cam->v4l2_lock);
   2348		wait_event_interruptible(cam->wq_stream,
   2349			       !video_is_registered(&cam->vdev) ||
   2350			       (frame = cam->curbuff)->status == FRAME_READY);
   2351		mutex_lock(&cam->v4l2_lock);
   2352		if (signal_pending(current))
   2353			return -ERESTARTSYS;
   2354		if (!video_is_registered(&cam->vdev))
   2355			return 0;
   2356	}
   2357
   2358	/* copy data to user space */
   2359	if (frame->length > count)
   2360		return -EFAULT;
   2361	if (copy_to_user(buf, frame->data, frame->length))
   2362		return -EFAULT;
   2363
   2364	count = frame->length;
   2365
   2366	frame->status = FRAME_EMPTY;
   2367
   2368	return count;
   2369}
   2370
   2371/******************************************************************************
   2372 *
   2373 *  cpia2_poll
   2374 *
   2375 *****************************************************************************/
   2376__poll_t cpia2_poll(struct camera_data *cam, struct file *filp,
   2377			poll_table *wait)
   2378{
   2379	__poll_t status = v4l2_ctrl_poll(filp, wait);
   2380
   2381	if ((poll_requested_events(wait) & (EPOLLIN | EPOLLRDNORM)) &&
   2382			!cam->streaming) {
   2383		/* Start streaming */
   2384		cpia2_usb_stream_start(cam,
   2385				       cam->params.camera_state.stream_mode);
   2386	}
   2387
   2388	poll_wait(filp, &cam->wq_stream, wait);
   2389
   2390	if (cam->curbuff->status == FRAME_READY)
   2391		status |= EPOLLIN | EPOLLRDNORM;
   2392
   2393	return status;
   2394}
   2395
   2396/******************************************************************************
   2397 *
   2398 *  cpia2_remap_buffer
   2399 *
   2400 *****************************************************************************/
   2401int cpia2_remap_buffer(struct camera_data *cam, struct vm_area_struct *vma)
   2402{
   2403	const char *adr = (const char *)vma->vm_start;
   2404	unsigned long size = vma->vm_end-vma->vm_start;
   2405	unsigned long start_offset = vma->vm_pgoff << PAGE_SHIFT;
   2406	unsigned long start = (unsigned long) adr;
   2407	unsigned long page, pos;
   2408
   2409	DBG("mmap offset:%ld size:%ld\n", start_offset, size);
   2410
   2411	if (!video_is_registered(&cam->vdev))
   2412		return -ENODEV;
   2413
   2414	if (size > cam->frame_size*cam->num_frames  ||
   2415	    (start_offset % cam->frame_size) != 0 ||
   2416	    (start_offset+size > cam->frame_size*cam->num_frames))
   2417		return -EINVAL;
   2418
   2419	pos = ((unsigned long) (cam->frame_buffer)) + start_offset;
   2420	while (size > 0) {
   2421		page = kvirt_to_pa(pos);
   2422		if (remap_pfn_range(vma, start, page >> PAGE_SHIFT, PAGE_SIZE, PAGE_SHARED))
   2423			return -EAGAIN;
   2424		start += PAGE_SIZE;
   2425		pos += PAGE_SIZE;
   2426		if (size > PAGE_SIZE)
   2427			size -= PAGE_SIZE;
   2428		else
   2429			size = 0;
   2430	}
   2431
   2432	cam->mmapped = true;
   2433	return 0;
   2434}