Logo Search packages:      
Sourcecode: linux version File versions  Download package

cpia2_core.c

/****************************************************************************
 *
 *  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);
      }

      schedule_timeout_interruptible(msecs_to_jiffies(100));

      if (cam->params.pnp_id.device_type == DEVICE_STV_672)
            retval = apply_vp_patch(cam);

      /* wait for vp to go to sleep */
      schedule_timeout_interruptible(msecs_to_jiffies(100));

      /***
       * 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 */
      schedule_timeout_interruptible(msecs_to_jiffies(100));

      set_all_properties(cam);

      cpia2_do_command(cam, CPIA2_CMD_GET_USER_MODE, TRANSFER_READ, 0);
      DBG("After SetAllProperties(cam), user mode is 0x%0X\n",
          cam->params.vp_params.video_mode);

      /***
       * Set audio regulator off.  This and the code to set the compresison
       * state are too complex to form a CPIA2_CMD_, and seem to be somewhat
       * intertwined.  This stuff came straight from the windows driver.
       ***/
      /* Turn AutoExposure off in VP and enable the serial bridge to the sensor */
      cpia2_do_command(cam, CPIA2_CMD_GET_VP_SYSTEM_CTRL, TRANSFER_READ, 0);
      tmp_reg = cam->params.vp_params.system_ctrl;
      cmd.buffer.registers[0].value = tmp_reg &
            (tmp_reg & (CPIA2_VP_SYSTEMCTRL_HK_CONTROL ^ 0xFF));

      cpia2_do_command(cam, CPIA2_CMD_GET_DEVICE_CONFIG, TRANSFER_READ, 0);
      cmd.buffer.registers[1].value = cam->params.vp_params.device_config |
                              CPIA2_VP_DEVICE_CONFIG_SERIAL_BRIDGE;
      cmd.buffer.registers[0].index = CPIA2_VP_SYSTEMCTRL;
      cmd.buffer.registers[1].index = CPIA2_VP_DEVICE_CONFIG;
      cmd.req_mode = CAMERAACCESS_TYPE_RANDOM | CAMERAACCESS_VP;
      cmd.reg_count = 2;
      cmd.direction = TRANSFER_WRITE;
      cmd.start = 0;
      cpia2_send_command(cam, &cmd);

      /* Set the correct I2C address in the CPiA-2 system register */
      cpia2_do_command(cam,
                   CPIA2_CMD_SET_SERIAL_ADDR,
                   TRANSFER_WRITE,
                   CPIA2_SYSTEM_VP_SERIAL_ADDR_SENSOR);

      /* Now have sensor access - set bit to turn the audio regulator off */
      cpia2_do_command(cam,
                   CPIA2_CMD_SET_SENSOR_CR1,
                   TRANSFER_WRITE, CPIA2_SENSOR_CR1_DOWN_AUDIO_REGULATOR);

      /* Set the correct I2C address in the CPiA-2 system register */
      if (cam->params.pnp_id.device_type == DEVICE_STV_672)
            cpia2_do_command(cam,
                         CPIA2_CMD_SET_SERIAL_ADDR,
                         TRANSFER_WRITE,
                         CPIA2_SYSTEM_VP_SERIAL_ADDR_VP); // 0x88
      else
            cpia2_do_command(cam,
                         CPIA2_CMD_SET_SERIAL_ADDR,
                         TRANSFER_WRITE,
                         CPIA2_SYSTEM_VP_SERIAL_ADDR_676_VP); // 0x8a

      /* increase signal drive strength */
      if (cam->params.pnp_id.device_type == DEVICE_STV_676)
            cpia2_do_command(cam,
                         CPIA2_CMD_SET_VP_EXP_MODES,
                         TRANSFER_WRITE,
                         CPIA2_VP_EXPOSURE_MODES_COMPILE_EXP);

      /* Start autoexposure */
      cpia2_do_command(cam, CPIA2_CMD_GET_DEVICE_CONFIG, TRANSFER_READ, 0);
      cmd.buffer.registers[0].value = cam->params.vp_params.device_config &
                          (CPIA2_VP_DEVICE_CONFIG_SERIAL_BRIDGE ^ 0xFF);

      cpia2_do_command(cam, CPIA2_CMD_GET_VP_SYSTEM_CTRL, TRANSFER_READ, 0);
      cmd.buffer.registers[1].value =
          cam->params.vp_params.system_ctrl | CPIA2_VP_SYSTEMCTRL_HK_CONTROL;

      cmd.buffer.registers[0].index = CPIA2_VP_DEVICE_CONFIG;
      cmd.buffer.registers[1].index = CPIA2_VP_SYSTEMCTRL;
      cmd.req_mode = CAMERAACCESS_TYPE_RANDOM | CAMERAACCESS_VP;
      cmd.reg_count = 2;
      cmd.direction = TRANSFER_WRITE;

      cpia2_send_command(cam, &cmd);

      /* Set compression state */
      cpia2_do_command(cam, CPIA2_CMD_GET_VC_CONTROL, TRANSFER_READ, 0);
      if (cam->params.compression.inhibit_htables) {
            tmp_reg = cam->params.vc_params.vc_control |
                    CPIA2_VC_VC_CTRL_INHIBIT_H_TABLES;
      } else  {
            tmp_reg = cam->params.vc_params.vc_control &
                    ~CPIA2_VC_VC_CTRL_INHIBIT_H_TABLES;
      }
      cpia2_do_command(cam, CPIA2_CMD_SET_VC_CONTROL, TRANSFER_WRITE,tmp_reg);

      /* Set target size (kb) on vc */
      cpia2_do_command(cam, CPIA2_CMD_SET_TARGET_KB,
                   TRANSFER_WRITE, cam->params.vc_params.target_kb);

      /* Wiggle VC Reset */
      /***
       * First read and wait a bit.
       ***/
      for (i = 0; i < 50; i++) {
            cpia2_do_command(cam, CPIA2_CMD_GET_PW_CONTROL,
                         TRANSFER_READ, 0);
      }

      tmp_reg = cam->params.vc_params.pw_control;
      tmp_reg &= ~CPIA2_VC_PW_CTRL_VC_RESET_N;

      cpia2_do_command(cam, CPIA2_CMD_SET_PW_CONTROL, TRANSFER_WRITE,tmp_reg);

      tmp_reg |= CPIA2_VC_PW_CTRL_VC_RESET_N;
      cpia2_do_command(cam, CPIA2_CMD_SET_PW_CONTROL, TRANSFER_WRITE,tmp_reg);

      cpia2_do_command(cam, CPIA2_CMD_SET_DEF_JPEG_OPT, TRANSFER_WRITE, 0);

      cpia2_do_command(cam, CPIA2_CMD_GET_USER_MODE, TRANSFER_READ, 0);
      DBG("After VC RESET, user mode is 0x%0X\n",
          cam->params.vp_params.video_mode);

      return retval;
}

/******************************************************************************
 *
 *  cpia2_set_high_power
 *
 *****************************************************************************/
static int cpia2_set_high_power(struct camera_data *cam)
{
      int i;
      for (i = 0; i <= 50; i++) {
            /* Read system status */
            cpia2_do_command(cam,CPIA2_CMD_GET_SYSTEM_CTRL,TRANSFER_READ,0);

            /* If there is an error, clear it */
            if(cam->params.camera_state.system_ctrl &
               CPIA2_SYSTEM_CONTROL_V2W_ERR)
                  cpia2_do_command(cam, CPIA2_CMD_CLEAR_V2W_ERR,
                               TRANSFER_WRITE, 0);

            /* Try to set high power mode */
            cpia2_do_command(cam, CPIA2_CMD_SET_SYSTEM_CTRL,
                         TRANSFER_WRITE, 1);

            /* Try to read something in VP to check if everything is awake */
            cpia2_do_command(cam, CPIA2_CMD_GET_VP_SYSTEM_STATE,
                         TRANSFER_READ, 0);
            if (cam->params.vp_params.system_state &
                CPIA2_VP_SYSTEMSTATE_HK_ALIVE) {
                  break;
            } else if (i == 50) {
                  cam->params.camera_state.power_mode = LO_POWER_MODE;
                  ERR("Camera did not wake up\n");
                  return -EIO;
            }
      }

      DBG("System now in high power state\n");
      cam->params.camera_state.power_mode = HI_POWER_MODE;
      return 0;
}

/******************************************************************************
 *
 *  cpia2_set_low_power
 *
 *****************************************************************************/
int cpia2_set_low_power(struct camera_data *cam)
{
      cam->params.camera_state.power_mode = LO_POWER_MODE;
      cpia2_do_command(cam, CPIA2_CMD_SET_SYSTEM_CTRL, TRANSFER_WRITE, 0);
      return 0;
}

/******************************************************************************
 *
 *  apply_vp_patch
 *
 *****************************************************************************/
static int apply_vp_patch(struct camera_data *cam)
{
      int i, j;
      struct cpia2_command cmd;

      cmd.req_mode = CAMERAACCESS_TYPE_REPEAT | CAMERAACCESS_VP;
      cmd.direction = TRANSFER_WRITE;

      for (i = 0; i < PATCH_DATA_SIZE; i++) {
            for (j = 0; j < patch_data[i].count; j++) {
                  cmd.buffer.block_data[j] = patch_data[i].data[j];
            }

            cmd.start = patch_data[i].reg;
            cmd.reg_count = patch_data[i].count;
            cpia2_send_command(cam, &cmd);
      }

      return 0;
}

/******************************************************************************
 *
 *  set_default_user_mode
 *
 *****************************************************************************/
static int set_default_user_mode(struct camera_data *cam)
{
      unsigned char user_mode;
      unsigned char frame_rate;
      int width = cam->params.roi.width;
      int height = cam->params.roi.height;

      switch (cam->params.version.sensor_flags) {
      case CPIA2_VP_SENSOR_FLAGS_404:
      case CPIA2_VP_SENSOR_FLAGS_407:
      case CPIA2_VP_SENSOR_FLAGS_409:
      case CPIA2_VP_SENSOR_FLAGS_410:
            if ((width > STV_IMAGE_QCIF_COLS)
                || (height > STV_IMAGE_QCIF_ROWS)) {
                  user_mode = CPIA2_VP_USER_MODE_CIF;
            } else {
                  user_mode = CPIA2_VP_USER_MODE_QCIFDS;
            }
            frame_rate = CPIA2_VP_FRAMERATE_30;
            break;
      case CPIA2_VP_SENSOR_FLAGS_500:
            if ((width > STV_IMAGE_CIF_COLS)
                || (height > STV_IMAGE_CIF_ROWS)) {
                  user_mode = CPIA2_VP_USER_MODE_VGA;
            } else {
                  user_mode = CPIA2_VP_USER_MODE_QVGADS;
            }
            if (cam->params.pnp_id.device_type == DEVICE_STV_672)
                  frame_rate = CPIA2_VP_FRAMERATE_15;
            else
                  frame_rate = CPIA2_VP_FRAMERATE_30;
            break;
      default:
            LOG("%s: Invalid sensor flag value 0x%0X\n",__FUNCTION__,
                cam->params.version.sensor_flags);
            return -EINVAL;
      }

      DBG("Sensor flag = 0x%0x, user mode = 0x%0x, frame rate = 0x%X\n",
          cam->params.version.sensor_flags, user_mode, frame_rate);
      cpia2_do_command(cam, CPIA2_CMD_SET_USER_MODE, TRANSFER_WRITE,
                   user_mode);
      if(cam->params.vp_params.frame_rate > 0 &&
         frame_rate > cam->params.vp_params.frame_rate)
            frame_rate = cam->params.vp_params.frame_rate;

      cpia2_set_fps(cam, frame_rate);

//    if (cam->params.pnp_id.device_type == DEVICE_STV_676)
//          cpia2_do_command(cam,
//                       CPIA2_CMD_SET_VP_SYSTEM_CTRL,
//                       TRANSFER_WRITE,
//                       CPIA2_VP_SYSTEMCTRL_HK_CONTROL |
//                       CPIA2_VP_SYSTEMCTRL_POWER_CONTROL);

      return 0;
}

/******************************************************************************
 *
 *  cpia2_match_video_size
 *
 *  return the best match, where 'best' is as always
 *  the largest that is not bigger than what is requested.
 *****************************************************************************/
int cpia2_match_video_size(int width, int height)
{
      if (width >= STV_IMAGE_VGA_COLS && height >= STV_IMAGE_VGA_ROWS)
            return VIDEOSIZE_VGA;

      if (width >= STV_IMAGE_CIF_COLS && height >= STV_IMAGE_CIF_ROWS)
            return VIDEOSIZE_CIF;

      if (width >= STV_IMAGE_QVGA_COLS && height >= STV_IMAGE_QVGA_ROWS)
            return VIDEOSIZE_QVGA;

      if (width >= 288 && height >= 216)
            return VIDEOSIZE_288_216;

      if (width >= 256 && height >= 192)
            return VIDEOSIZE_256_192;

      if (width >= 224 && height >= 168)
            return VIDEOSIZE_224_168;

      if (width >= 192 && height >= 144)
            return VIDEOSIZE_192_144;

      if (width >= STV_IMAGE_QCIF_COLS && height >= STV_IMAGE_QCIF_ROWS)
            return VIDEOSIZE_QCIF;

      return -1;
}

/******************************************************************************
 *
 *  SetVideoSize
 *
 *****************************************************************************/
static int set_vw_size(struct camera_data *cam, int size)
{
      int retval = 0;

      cam->params.vp_params.video_size = size;

      switch (size) {
      case VIDEOSIZE_VGA:
            DBG("Setting size to VGA\n");
            cam->params.roi.width = STV_IMAGE_VGA_COLS;
            cam->params.roi.height = STV_IMAGE_VGA_ROWS;
            cam->vw.width = STV_IMAGE_VGA_COLS;
            cam->vw.height = STV_IMAGE_VGA_ROWS;
            break;
      case VIDEOSIZE_CIF:
            DBG("Setting size to CIF\n");
            cam->params.roi.width = STV_IMAGE_CIF_COLS;
            cam->params.roi.height = STV_IMAGE_CIF_ROWS;
            cam->vw.width = STV_IMAGE_CIF_COLS;
            cam->vw.height = STV_IMAGE_CIF_ROWS;
            break;
      case VIDEOSIZE_QVGA:
            DBG("Setting size to QVGA\n");
            cam->params.roi.width = STV_IMAGE_QVGA_COLS;
            cam->params.roi.height = STV_IMAGE_QVGA_ROWS;
            cam->vw.width = STV_IMAGE_QVGA_COLS;
            cam->vw.height = STV_IMAGE_QVGA_ROWS;
            break;
      case VIDEOSIZE_288_216:
            cam->params.roi.width = 288;
            cam->params.roi.height = 216;
            cam->vw.width = 288;
            cam->vw.height = 216;
            break;
      case VIDEOSIZE_256_192:
            cam->vw.width = 256;
            cam->vw.height = 192;
            cam->params.roi.width = 256;
            cam->params.roi.height = 192;
            break;
      case VIDEOSIZE_224_168:
            cam->vw.width = 224;
            cam->vw.height = 168;
            cam->params.roi.width = 224;
            cam->params.roi.height = 168;
            break;
      case VIDEOSIZE_192_144:
            cam->vw.width = 192;
            cam->vw.height = 144;
            cam->params.roi.width = 192;
            cam->params.roi.height = 144;
            break;
      case VIDEOSIZE_QCIF:
            DBG("Setting size to QCIF\n");
            cam->params.roi.width = STV_IMAGE_QCIF_COLS;
            cam->params.roi.height = STV_IMAGE_QCIF_ROWS;
            cam->vw.width = STV_IMAGE_QCIF_COLS;
            cam->vw.height = STV_IMAGE_QCIF_ROWS;
            break;
      default:
            retval = -EINVAL;
      }
      return retval;
}

/******************************************************************************
 *
 *  configure_sensor
 *
 *****************************************************************************/
static int configure_sensor(struct camera_data *cam,
                      int req_width, int req_height)
{
      int retval;

      switch (cam->params.version.sensor_flags) {
      case CPIA2_VP_SENSOR_FLAGS_404:
      case CPIA2_VP_SENSOR_FLAGS_407:
      case CPIA2_VP_SENSOR_FLAGS_409:
      case CPIA2_VP_SENSOR_FLAGS_410:
            retval = config_sensor_410(cam, req_width, req_height);
            break;
      case CPIA2_VP_SENSOR_FLAGS_500:
            retval = config_sensor_500(cam, req_width, req_height);
            break;
      default:
            return -EINVAL;
      }

      return retval;
}

/******************************************************************************
 *
 *  config_sensor_410
 *
 *****************************************************************************/
static int config_sensor_410(struct camera_data *cam,
                      int req_width, int req_height)
{
      struct cpia2_command cmd;
      int i = 0;
      int image_size;
      int image_type;
      int width = req_width;
      int height = req_height;

      /***
       *  Make sure size doesn't exceed CIF.
       ***/
      if (width > STV_IMAGE_CIF_COLS)
            width = STV_IMAGE_CIF_COLS;
      if (height > STV_IMAGE_CIF_ROWS)
            height = STV_IMAGE_CIF_ROWS;

      image_size = cpia2_match_video_size(width, height);

      DBG("Config 410: width = %d, height = %d\n", width, height);
      DBG("Image size returned is %d\n", image_size);
      if (image_size >= 0) {
            set_vw_size(cam, image_size);
            width = cam->params.roi.width;
            height = cam->params.roi.height;

            DBG("After set_vw_size(), width = %d, height = %d\n",
                width, height);
            if (width <= 176 && height <= 144) {
                  DBG("image type = VIDEOSIZE_QCIF\n");
                  image_type = VIDEOSIZE_QCIF;
            }
            else if (width <= 320 && height <= 240) {
                  DBG("image type = VIDEOSIZE_QVGA\n");
                  image_type = VIDEOSIZE_QVGA;
            }
            else {
                  DBG("image type = VIDEOSIZE_CIF\n");
                  image_type = VIDEOSIZE_CIF;
            }
      } else {
            ERR("ConfigSensor410 failed\n");
            return -EINVAL;
      }

      cmd.req_mode = CAMERAACCESS_TYPE_RANDOM | CAMERAACCESS_VC;
      cmd.direction = TRANSFER_WRITE;

      /* VC Format */
      cmd.buffer.registers[i].index = CPIA2_VC_VC_FORMAT;
      if (image_type == VIDEOSIZE_CIF) {
            cmd.buffer.registers[i++].value =
                (u8) (CPIA2_VC_VC_FORMAT_UFIRST |
                    CPIA2_VC_VC_FORMAT_SHORTLINE);
      } else {
            cmd.buffer.registers[i++].value =
                (u8) CPIA2_VC_VC_FORMAT_UFIRST;
      }

      /* VC Clocks */
      cmd.buffer.registers[i].index = CPIA2_VC_VC_CLOCKS;
      if (image_type == VIDEOSIZE_QCIF) {
            if (cam->params.pnp_id.device_type == DEVICE_STV_672) {
                  cmd.buffer.registers[i++].value=
                        (u8)(CPIA2_VC_VC_672_CLOCKS_CIF_DIV_BY_3 |
                             CPIA2_VC_VC_672_CLOCKS_SCALING |
                             CPIA2_VC_VC_CLOCKS_LOGDIV2);
                  DBG("VC_Clocks (0xc4) should be B\n");
            }
            else {
                  cmd.buffer.registers[i++].value=
                        (u8)(CPIA2_VC_VC_676_CLOCKS_CIF_DIV_BY_3 |
                             CPIA2_VC_VC_CLOCKS_LOGDIV2);
            }
      } else {
            if (cam->params.pnp_id.device_type == DEVICE_STV_672) {
                  cmd.buffer.registers[i++].value =
                     (u8) (CPIA2_VC_VC_672_CLOCKS_CIF_DIV_BY_3 |
                         CPIA2_VC_VC_CLOCKS_LOGDIV0);
            }
            else {
                  cmd.buffer.registers[i++].value =
                     (u8) (CPIA2_VC_VC_676_CLOCKS_CIF_DIV_BY_3 |
                         CPIA2_VC_VC_676_CLOCKS_SCALING |
                         CPIA2_VC_VC_CLOCKS_LOGDIV0);
            }
      }
      DBG("VC_Clocks (0xc4) = 0x%0X\n", cmd.buffer.registers[i-1].value);

      /* Input reqWidth from VC */
      cmd.buffer.registers[i].index = CPIA2_VC_VC_IHSIZE_LO;
      if (image_type == VIDEOSIZE_QCIF)
            cmd.buffer.registers[i++].value =
                (u8) (STV_IMAGE_QCIF_COLS / 4);
      else
            cmd.buffer.registers[i++].value =
                (u8) (STV_IMAGE_CIF_COLS / 4);

      /* Timings */
      cmd.buffer.registers[i].index = CPIA2_VC_VC_XLIM_HI;
      if (image_type == VIDEOSIZE_QCIF)
            cmd.buffer.registers[i++].value = (u8) 0;
      else
            cmd.buffer.registers[i++].value = (u8) 1;

      cmd.buffer.registers[i].index = CPIA2_VC_VC_XLIM_LO;
      if (image_type == VIDEOSIZE_QCIF)
            cmd.buffer.registers[i++].value = (u8) 208;
      else
            cmd.buffer.registers[i++].value = (u8) 160;

      cmd.buffer.registers[i].index = CPIA2_VC_VC_YLIM_HI;
      if (image_type == VIDEOSIZE_QCIF)
            cmd.buffer.registers[i++].value = (u8) 0;
      else
            cmd.buffer.registers[i++].value = (u8) 1;

      cmd.buffer.registers[i].index = CPIA2_VC_VC_YLIM_LO;
      if (image_type == VIDEOSIZE_QCIF)
            cmd.buffer.registers[i++].value = (u8) 160;
      else
            cmd.buffer.registers[i++].value = (u8) 64;

      /* Output Image Size */
      cmd.buffer.registers[i].index = CPIA2_VC_VC_OHSIZE;
      cmd.buffer.registers[i++].value = cam->params.roi.width / 4;

      cmd.buffer.registers[i].index = CPIA2_VC_VC_OVSIZE;
      cmd.buffer.registers[i++].value = cam->params.roi.height / 4;

      /* Cropping */
      cmd.buffer.registers[i].index = CPIA2_VC_VC_HCROP;
      if (image_type == VIDEOSIZE_QCIF)
            cmd.buffer.registers[i++].value =
                (u8) (((STV_IMAGE_QCIF_COLS / 4) - (width / 4)) / 2);
      else
            cmd.buffer.registers[i++].value =
                (u8) (((STV_IMAGE_CIF_COLS / 4) - (width / 4)) / 2);

      cmd.buffer.registers[i].index = CPIA2_VC_VC_VCROP;
      if (image_type == VIDEOSIZE_QCIF)
            cmd.buffer.registers[i++].value =
                (u8) (((STV_IMAGE_QCIF_ROWS / 4) - (height / 4)) / 2);
      else
            cmd.buffer.registers[i++].value =
                (u8) (((STV_IMAGE_CIF_ROWS / 4) - (height / 4)) / 2);

      /* Scaling registers (defaults) */
      cmd.buffer.registers[i].index = CPIA2_VC_VC_HPHASE;
      cmd.buffer.registers[i++].value = (u8) 0;

      cmd.buffer.registers[i].index = CPIA2_VC_VC_VPHASE;
      cmd.buffer.registers[i++].value = (u8) 0;

      cmd.buffer.registers[i].index = CPIA2_VC_VC_HISPAN;
      cmd.buffer.registers[i++].value = (u8) 31;

      cmd.buffer.registers[i].index = CPIA2_VC_VC_VISPAN;
      cmd.buffer.registers[i++].value = (u8) 31;

      cmd.buffer.registers[i].index = CPIA2_VC_VC_HICROP;
      cmd.buffer.registers[i++].value = (u8) 0;

      cmd.buffer.registers[i].index = CPIA2_VC_VC_VICROP;
      cmd.buffer.registers[i++].value = (u8) 0;

      cmd.buffer.registers[i].index = CPIA2_VC_VC_HFRACT;
      cmd.buffer.registers[i++].value = (u8) 0x81;    /* = 8/1 = 8 (HIBYTE/LOBYTE) */

      cmd.buffer.registers[i].index = CPIA2_VC_VC_VFRACT;
      cmd.buffer.registers[i++].value = (u8) 0x81;    /* = 8/1 = 8 (HIBYTE/LOBYTE) */

      cmd.reg_count = i;

      cpia2_send_command(cam, &cmd);

      return i;
}


/******************************************************************************
 *
 *  config_sensor_500(cam)
 *
 *****************************************************************************/
static int config_sensor_500(struct camera_data *cam,
                       int req_width, int req_height)
{
      struct cpia2_command cmd;
      int i = 0;
      int image_size = VIDEOSIZE_CIF;
      int image_type = VIDEOSIZE_VGA;
      int width = req_width;
      int height = req_height;
      unsigned int device = cam->params.pnp_id.device_type;

      image_size = cpia2_match_video_size(width, height);

      if (width > STV_IMAGE_CIF_COLS || height > STV_IMAGE_CIF_ROWS)
            image_type = VIDEOSIZE_VGA;
      else if (width > STV_IMAGE_QVGA_COLS || height > STV_IMAGE_QVGA_ROWS)
            image_type = VIDEOSIZE_CIF;
      else if (width > STV_IMAGE_QCIF_COLS || height > STV_IMAGE_QCIF_ROWS)
            image_type = VIDEOSIZE_QVGA;
      else
            image_type = VIDEOSIZE_QCIF;

      if (image_size >= 0) {
            set_vw_size(cam, image_size);
            width = cam->params.roi.width;
            height = cam->params.roi.height;
      } else {
            ERR("ConfigSensor500 failed\n");
            return -EINVAL;
      }

      DBG("image_size = %d, width = %d, height = %d, type = %d\n",
          image_size, width, height, image_type);

      cmd.req_mode = CAMERAACCESS_TYPE_RANDOM | CAMERAACCESS_VC;
      cmd.direction = TRANSFER_WRITE;
      i = 0;

      /* VC Format */
      cmd.buffer.registers[i].index = CPIA2_VC_VC_FORMAT;
      cmd.buffer.registers[i].value = (u8) CPIA2_VC_VC_FORMAT_UFIRST;
      if (image_type == VIDEOSIZE_QCIF)
            cmd.buffer.registers[i].value |= (u8) CPIA2_VC_VC_FORMAT_DECIMATING;
      i++;

      /* VC Clocks */
      cmd.buffer.registers[i].index = CPIA2_VC_VC_CLOCKS;
      if (device == DEVICE_STV_672) {
            if (image_type == VIDEOSIZE_VGA)
                  cmd.buffer.registers[i].value =
                        (u8)CPIA2_VC_VC_CLOCKS_LOGDIV1;
            else
                  cmd.buffer.registers[i].value =
                        (u8)(CPIA2_VC_VC_672_CLOCKS_SCALING |
                             CPIA2_VC_VC_CLOCKS_LOGDIV3);
      } else {
            if (image_type == VIDEOSIZE_VGA)
                  cmd.buffer.registers[i].value =
                        (u8)CPIA2_VC_VC_CLOCKS_LOGDIV0;
            else
                  cmd.buffer.registers[i].value =
                        (u8)(CPIA2_VC_VC_676_CLOCKS_SCALING |
                             CPIA2_VC_VC_CLOCKS_LOGDIV2);
      }
      i++;

      DBG("VC_CLOCKS = 0x%X\n", cmd.buffer.registers[i-1].value);

      /* Input width from VP */
      cmd.buffer.registers[i].index = CPIA2_VC_VC_IHSIZE_LO;
      if (image_type == VIDEOSIZE_VGA)
            cmd.buffer.registers[i].value =
                (u8) (STV_IMAGE_VGA_COLS / 4);
      else
            cmd.buffer.registers[i].value =
                (u8) (STV_IMAGE_QVGA_COLS / 4);
      i++;
      DBG("Input width = %d\n", cmd.buffer.registers[i-1].value);

      /* Timings */
      cmd.buffer.registers[i].index = CPIA2_VC_VC_XLIM_HI;
      if (image_type == VIDEOSIZE_VGA)
            cmd.buffer.registers[i++].value = (u8) 2;
      else
            cmd.buffer.registers[i++].value = (u8) 1;

      cmd.buffer.registers[i].index = CPIA2_VC_VC_XLIM_LO;
      if (image_type == VIDEOSIZE_VGA)
            cmd.buffer.registers[i++].value = (u8) 250;
      else if (image_type == VIDEOSIZE_QVGA)
            cmd.buffer.registers[i++].value = (u8) 125;
      else
            cmd.buffer.registers[i++].value = (u8) 160;

      cmd.buffer.registers[i].index = CPIA2_VC_VC_YLIM_HI;
      if (image_type == VIDEOSIZE_VGA)
            cmd.buffer.registers[i++].value = (u8) 2;
      else
            cmd.buffer.registers[i++].value = (u8) 1;

      cmd.buffer.registers[i].index = CPIA2_VC_VC_YLIM_LO;
      if (image_type == VIDEOSIZE_VGA)
            cmd.buffer.registers[i++].value = (u8) 12;
      else if (image_type == VIDEOSIZE_QVGA)
            cmd.buffer.registers[i++].value = (u8) 64;
      else
            cmd.buffer.registers[i++].value = (u8) 6;

      /* Output Image Size */
      cmd.buffer.registers[i].index = CPIA2_VC_VC_OHSIZE;
      if (image_type == VIDEOSIZE_QCIF)
            cmd.buffer.registers[i++].value = STV_IMAGE_CIF_COLS  / 4;
      else
            cmd.buffer.registers[i++].value = width / 4;

      cmd.buffer.registers[i].index = CPIA2_VC_VC_OVSIZE;
      if (image_type == VIDEOSIZE_QCIF)
            cmd.buffer.registers[i++].value = STV_IMAGE_CIF_ROWS  / 4;
      else
            cmd.buffer.registers[i++].value = height / 4;

      /* Cropping */
      cmd.buffer.registers[i].index = CPIA2_VC_VC_HCROP;
      if (image_type == VIDEOSIZE_VGA)
            cmd.buffer.registers[i++].value =
                (u8) (((STV_IMAGE_VGA_COLS / 4) - (width / 4)) / 2);
      else if (image_type == VIDEOSIZE_QVGA)
            cmd.buffer.registers[i++].value =
                (u8) (((STV_IMAGE_QVGA_COLS / 4) - (width / 4)) / 2);
      else if (image_type == VIDEOSIZE_CIF)
            cmd.buffer.registers[i++].value =
                (u8) (((STV_IMAGE_CIF_COLS / 4) - (width / 4)) / 2);
      else /*if (image_type == VIDEOSIZE_QCIF)*/
            cmd.buffer.registers[i++].value =
                  (u8) (((STV_IMAGE_QCIF_COLS / 4) - (width / 4)) / 2);

      cmd.buffer.registers[i].index = CPIA2_VC_VC_VCROP;
      if (image_type == VIDEOSIZE_VGA)
            cmd.buffer.registers[i++].value =
                (u8) (((STV_IMAGE_VGA_ROWS / 4) - (height / 4)) / 2);
      else if (image_type == VIDEOSIZE_QVGA)
            cmd.buffer.registers[i++].value =
                (u8) (((STV_IMAGE_QVGA_ROWS / 4) - (height / 4)) / 2);
      else if (image_type == VIDEOSIZE_CIF)
            cmd.buffer.registers[i++].value =
                (u8) (((STV_IMAGE_CIF_ROWS / 4) - (height / 4)) / 2);
      else /*if (image_type == VIDEOSIZE_QCIF)*/
            cmd.buffer.registers[i++].value =
                (u8) (((STV_IMAGE_QCIF_ROWS / 4) - (height / 4)) / 2);

      /* Scaling registers (defaults) */
      cmd.buffer.registers[i].index = CPIA2_VC_VC_HPHASE;
      if (image_type == VIDEOSIZE_CIF || image_type == VIDEOSIZE_QCIF)
            cmd.buffer.registers[i++].value = (u8) 36;
      else
            cmd.buffer.registers[i++].value = (u8) 0;

      cmd.buffer.registers[i].index = CPIA2_VC_VC_VPHASE;
      if (image_type == VIDEOSIZE_CIF || image_type == VIDEOSIZE_QCIF)
            cmd.buffer.registers[i++].value = (u8) 32;
      else
            cmd.buffer.registers[i++].value = (u8) 0;

      cmd.buffer.registers[i].index = CPIA2_VC_VC_HISPAN;
      if (image_type == VIDEOSIZE_CIF || image_type == VIDEOSIZE_QCIF)
            cmd.buffer.registers[i++].value = (u8) 26;
      else
            cmd.buffer.registers[i++].value = (u8) 31;

      cmd.buffer.registers[i].index = CPIA2_VC_VC_VISPAN;
      if (image_type == VIDEOSIZE_CIF || image_type == VIDEOSIZE_QCIF)
            cmd.buffer.registers[i++].value = (u8) 21;
      else
            cmd.buffer.registers[i++].value = (u8) 31;

      cmd.buffer.registers[i].index = CPIA2_VC_VC_HICROP;
      cmd.buffer.registers[i++].value = (u8) 0;

      cmd.buffer.registers[i].index = CPIA2_VC_VC_VICROP;
      cmd.buffer.registers[i++].value = (u8) 0;

      cmd.buffer.registers[i].index = CPIA2_VC_VC_HFRACT;
      if (image_type == VIDEOSIZE_CIF || image_type == VIDEOSIZE_QCIF)
            cmd.buffer.registers[i++].value = (u8) 0x2B;    /* 2/11 */
      else
            cmd.buffer.registers[i++].value = (u8) 0x81;    /* 8/1 */

      cmd.buffer.registers[i].index = CPIA2_VC_VC_VFRACT;
      if (image_type == VIDEOSIZE_CIF || image_type == VIDEOSIZE_QCIF)
            cmd.buffer.registers[i++].value = (u8) 0x13;    /* 1/3 */
      else
            cmd.buffer.registers[i++].value = (u8) 0x81;    /* 8/1 */

      cmd.reg_count = i;

      cpia2_send_command(cam, &cmd);

      return i;
}


/******************************************************************************
 *
 *  setallproperties
 *
 *  This sets all user changeable properties to the values in cam->params.
 *****************************************************************************/
int set_all_properties(struct camera_data *cam)
{
      /**
       * Don't set target_kb here, it will be set later.
       * framerate and user_mode were already set (set_default_user_mode).
       **/

      cpia2_set_color_params(cam);

      cpia2_usb_change_streaming_alternate(cam,
                                cam->params.camera_state.stream_mode);

      cpia2_do_command(cam, CPIA2_CMD_SET_USER_EFFECTS, TRANSFER_WRITE,
                   cam->params.vp_params.user_effects);

      cpia2_set_flicker_mode(cam,
                         cam->params.flicker_control.flicker_mode_req);

      cpia2_do_command(cam,
                   CPIA2_CMD_SET_VC_MP_GPIO_DIRECTION,
                   TRANSFER_WRITE, cam->params.vp_params.gpio_direction);
      cpia2_do_command(cam, CPIA2_CMD_SET_VC_MP_GPIO_DATA, TRANSFER_WRITE,
                   cam->params.vp_params.gpio_data);

      wake_system(cam);

      set_lowlight_boost(cam);

      return 0;
}

/******************************************************************************
 *
 *  cpia2_save_camera_state
 *
 *****************************************************************************/
void cpia2_save_camera_state(struct camera_data *cam)
{
      get_color_params(cam);
      cpia2_do_command(cam, CPIA2_CMD_GET_USER_EFFECTS, TRANSFER_READ, 0);
      cpia2_do_command(cam, CPIA2_CMD_GET_VC_MP_GPIO_DIRECTION, TRANSFER_READ,
                   0);
      cpia2_do_command(cam, CPIA2_CMD_GET_VC_MP_GPIO_DATA, TRANSFER_READ, 0);
      /* Don't get framerate or target_kb. Trust the values we already have */
}

/******************************************************************************
 *
 *  get_color_params
 *
 *****************************************************************************/
void get_color_params(struct camera_data *cam)
{
      cpia2_do_command(cam, CPIA2_CMD_GET_VP_BRIGHTNESS, TRANSFER_READ, 0);
      cpia2_do_command(cam, CPIA2_CMD_GET_VP_SATURATION, TRANSFER_READ, 0);
      cpia2_do_command(cam, CPIA2_CMD_GET_CONTRAST, TRANSFER_READ, 0);
}

/******************************************************************************
 *
 *  cpia2_set_color_params
 *
 *****************************************************************************/
void cpia2_set_color_params(struct camera_data *cam)
{
      DBG("Setting color params\n");
      cpia2_set_brightness(cam, cam->params.color_params.brightness);
      cpia2_set_contrast(cam, cam->params.color_params.contrast);
      cpia2_set_saturation(cam, cam->params.color_params.saturation);
}

/******************************************************************************
 *
 *  cpia2_set_flicker_mode
 *
 *****************************************************************************/
int cpia2_set_flicker_mode(struct camera_data *cam, int mode)
{
      unsigned char cam_reg;
      int err = 0;

      if(cam->params.pnp_id.device_type != DEVICE_STV_672)
            return -EINVAL;

      /* Set the appropriate bits in FLICKER_MODES, preserving the rest */
      if((err = cpia2_do_command(cam, CPIA2_CMD_GET_FLICKER_MODES,
                           TRANSFER_READ, 0)))
            return err;
      cam_reg = cam->params.flicker_control.cam_register;

      switch(mode) {
      case NEVER_FLICKER:
            cam_reg |= CPIA2_VP_FLICKER_MODES_NEVER_FLICKER;
            cam_reg &= ~CPIA2_VP_FLICKER_MODES_50HZ;
            break;
      case FLICKER_60:
            cam_reg &= ~CPIA2_VP_FLICKER_MODES_NEVER_FLICKER;
            cam_reg &= ~CPIA2_VP_FLICKER_MODES_50HZ;
            break;
      case FLICKER_50:
            cam_reg &= ~CPIA2_VP_FLICKER_MODES_NEVER_FLICKER;
            cam_reg |= CPIA2_VP_FLICKER_MODES_50HZ;
            break;
      default:
            return -EINVAL;
      }

      if((err = cpia2_do_command(cam, CPIA2_CMD_SET_FLICKER_MODES,
                           TRANSFER_WRITE, cam_reg)))
            return err;

      /* Set the appropriate bits in EXP_MODES, preserving the rest */
      if((err = cpia2_do_command(cam, CPIA2_CMD_GET_VP_EXP_MODES,
                           TRANSFER_READ, 0)))
            return err;
      cam_reg = cam->params.vp_params.exposure_modes;

      if (mode == NEVER_FLICKER) {
            cam_reg |= CPIA2_VP_EXPOSURE_MODES_INHIBIT_FLICKER;
      } else {
            cam_reg &= ~CPIA2_VP_EXPOSURE_MODES_INHIBIT_FLICKER;
      }

      if((err = cpia2_do_command(cam, CPIA2_CMD_SET_VP_EXP_MODES,
                           TRANSFER_WRITE, cam_reg)))
            return err;

      if((err = cpia2_do_command(cam, CPIA2_CMD_REHASH_VP4,
                           TRANSFER_WRITE, 1)))
            return err;

      switch(mode) {
      case NEVER_FLICKER:
            cam->params.flicker_control.flicker_mode_req = mode;
            break;
      case FLICKER_60:
            cam->params.flicker_control.flicker_mode_req = mode;
            cam->params.flicker_control.mains_frequency = 60;
            break;
      case FLICKER_50:
            cam->params.flicker_control.flicker_mode_req = mode;
            cam->params.flicker_control.mains_frequency = 50;
            break;
      default:
            err = -EINVAL;
      }

      return err;
}

/******************************************************************************
 *
 *  cpia2_set_property_flip
 *
 *****************************************************************************/
void cpia2_set_property_flip(struct camera_data *cam, int prop_val)
{
      unsigned char cam_reg;

      cpia2_do_command(cam, CPIA2_CMD_GET_USER_EFFECTS, TRANSFER_READ, 0);
      cam_reg = cam->params.vp_params.user_effects;

      if (prop_val)
      {
            cam_reg |= CPIA2_VP_USER_EFFECTS_FLIP;
      }
      else
      {
            cam_reg &= ~CPIA2_VP_USER_EFFECTS_FLIP;
      }
      cpia2_do_command(cam, CPIA2_CMD_SET_USER_EFFECTS, TRANSFER_WRITE,
                   cam_reg);
}

/******************************************************************************
 *
 *  cpia2_set_property_mirror
 *
 *****************************************************************************/
void cpia2_set_property_mirror(struct camera_data *cam, int prop_val)
{
      unsigned char cam_reg;

      cpia2_do_command(cam, CPIA2_CMD_GET_USER_EFFECTS, TRANSFER_READ, 0);
      cam_reg = cam->params.vp_params.user_effects;

      if (prop_val)
      {
            cam_reg |= CPIA2_VP_USER_EFFECTS_MIRROR;
      }
      else
      {
            cam_reg &= ~CPIA2_VP_USER_EFFECTS_MIRROR;
      }
      cpia2_do_command(cam, CPIA2_CMD_SET_USER_EFFECTS, TRANSFER_WRITE,
                   cam_reg);
}

/******************************************************************************
 *
 *  set_target_kb
 *
 *  The new Target KB is set in cam->params.vc_params.target_kb and
 *  activates on reset.
 *****************************************************************************/

int cpia2_set_target_kb(struct camera_data *cam, unsigned char value)
{
      DBG("Requested target_kb = %d\n", value);
      if (value != cam->params.vc_params.target_kb) {

            cpia2_usb_stream_pause(cam);

            /* reset camera for new target_kb */
            cam->params.vc_params.target_kb = value;
            cpia2_reset_camera(cam);

            cpia2_usb_stream_resume(cam);
      }

      return 0;
}

/******************************************************************************
 *
 *  cpia2_set_gpio
 *
 *****************************************************************************/
int cpia2_set_gpio(struct camera_data *cam, unsigned char setting)
{
      int ret;

      /* Set the microport direction (register 0x90, should be defined
       * already) to 1 (user output), and set the microport data (0x91) to
       * the value in the ioctl argument.
       */

      ret = cpia2_do_command(cam,
                         CPIA2_CMD_SET_VC_MP_GPIO_DIRECTION,
                         CPIA2_VC_MP_DIR_OUTPUT,
                         255);
      if (ret < 0)
            return ret;
      cam->params.vp_params.gpio_direction = 255;

      ret = cpia2_do_command(cam,
                         CPIA2_CMD_SET_VC_MP_GPIO_DATA,
                         CPIA2_VC_MP_DIR_OUTPUT,
                         setting);
      if (ret < 0)
            return ret;
      cam->params.vp_params.gpio_data = setting;

      return 0;
}

/******************************************************************************
 *
 *  cpia2_set_fps
 *
 *****************************************************************************/
int cpia2_set_fps(struct camera_data *cam, int framerate)
{
      int retval;

      switch(framerate) {
            case CPIA2_VP_FRAMERATE_30:
            case CPIA2_VP_FRAMERATE_25:
                  if(cam->params.pnp_id.device_type == DEVICE_STV_672 &&
                     cam->params.version.sensor_flags ==
                                        CPIA2_VP_SENSOR_FLAGS_500) {
                        return -EINVAL;
                  }
                  /* Fall through */
            case CPIA2_VP_FRAMERATE_15:
            case CPIA2_VP_FRAMERATE_12_5:
            case CPIA2_VP_FRAMERATE_7_5:
            case CPIA2_VP_FRAMERATE_6_25:
                  break;
            default:
                  return -EINVAL;
      }

      if (cam->params.pnp_id.device_type == DEVICE_STV_672 &&
          framerate == CPIA2_VP_FRAMERATE_15)
            framerate = 0; /* Work around bug in VP4 */

      retval = cpia2_do_command(cam,
                         CPIA2_CMD_FRAMERATE_REQ,
                         TRANSFER_WRITE,
                         framerate);

      if(retval == 0)
            cam->params.vp_params.frame_rate = framerate;

      return retval;
}

/******************************************************************************
 *
 *  cpia2_set_brightness
 *
 *****************************************************************************/
void cpia2_set_brightness(struct camera_data *cam, unsigned char value)
{
      /***
       * Don't let the register be set to zero - bug in VP4 - flash of full
       * brightness
       ***/
      if (cam->params.pnp_id.device_type == DEVICE_STV_672 && value == 0)
            value++;
      DBG("Setting brightness to %d (0x%0x)\n", value, value);
      cpia2_do_command(cam,CPIA2_CMD_SET_VP_BRIGHTNESS, TRANSFER_WRITE,value);
}

/******************************************************************************
 *
 *  cpia2_set_contrast
 *
 *****************************************************************************/
void cpia2_set_contrast(struct camera_data *cam, unsigned char value)
{
      DBG("Setting contrast to %d (0x%0x)\n", value, value);
      cam->params.color_params.contrast = value;
      cpia2_do_command(cam, CPIA2_CMD_SET_CONTRAST, TRANSFER_WRITE, value);
}

/******************************************************************************
 *
 *  cpia2_set_saturation
 *
 *****************************************************************************/
void cpia2_set_saturation(struct camera_data *cam, unsigned char value)
{
      DBG("Setting saturation to %d (0x%0x)\n", value, value);
      cam->params.color_params.saturation = value;
      cpia2_do_command(cam,CPIA2_CMD_SET_VP_SATURATION, TRANSFER_WRITE,value);
}

/******************************************************************************
 *
 *  wake_system
 *
 *****************************************************************************/
void wake_system(struct camera_data *cam)
{
      cpia2_do_command(cam, CPIA2_CMD_SET_WAKEUP, TRANSFER_WRITE, 0);
}

/******************************************************************************
 *
 *  set_lowlight_boost
 *
 *  Valid for STV500 sensor only
 *****************************************************************************/
void set_lowlight_boost(struct camera_data *cam)
{
      struct cpia2_command cmd;

      if (cam->params.pnp_id.device_type != DEVICE_STV_672 ||
          cam->params.version.sensor_flags != CPIA2_VP_SENSOR_FLAGS_500)
            return;

      cmd.direction = TRANSFER_WRITE;
      cmd.req_mode = CAMERAACCESS_TYPE_BLOCK | CAMERAACCESS_VP;
      cmd.reg_count = 3;
      cmd.start = CPIA2_VP_RAM_ADDR_H;

      cmd.buffer.block_data[0] = 0; /* High byte of address to write to */
      cmd.buffer.block_data[1] = 0x59;    /* Low byte of address to write to */
      cmd.buffer.block_data[2] = 0; /* High byte of data to write */

      cpia2_send_command(cam, &cmd);

      if (cam->params.vp_params.lowlight_boost) {
            cmd.buffer.block_data[0] = 0x02;    /* Low byte data to write */
      } else {
            cmd.buffer.block_data[0] = 0x06;
      }
      cmd.start = CPIA2_VP_RAM_DATA;
      cmd.reg_count = 1;
      cpia2_send_command(cam, &cmd);

      /* Rehash the VP4 values */
      cpia2_do_command(cam, CPIA2_CMD_REHASH_VP4, TRANSFER_WRITE, 1);
}

/******************************************************************************
 *
 *  cpia2_set_format
 *
 *  Assumes that new size is already set in param struct.
 *****************************************************************************/
void cpia2_set_format(struct camera_data *cam)
{
      cam->flush = true;

      cpia2_usb_stream_pause(cam);

      /* reset camera to new size */
      cpia2_set_low_power(cam);
      cpia2_reset_camera(cam);
      cam->flush = false;

      cpia2_dbg_dump_registers(cam);

      cpia2_usb_stream_resume(cam);
}

/******************************************************************************
 *
 * cpia2_dbg_dump_registers
 *
 *****************************************************************************/
void cpia2_dbg_dump_registers(struct camera_data *cam)
{
#ifdef _CPIA2_DEBUG_
      struct cpia2_command cmd;

      if (!(debugs_on & DEBUG_DUMP_REGS))
            return;

      cmd.direction = TRANSFER_READ;

      /* Start with bank 0 (SYSTEM) */
      cmd.req_mode = CAMERAACCESS_TYPE_BLOCK | CAMERAACCESS_SYSTEM;
      cmd.reg_count = 3;
      cmd.start = 0;
      cpia2_send_command(cam, &cmd);
      printk(KERN_DEBUG "System Device Hi      = 0x%X\n",
             cmd.buffer.block_data[0]);
      printk(KERN_DEBUG "System Device Lo      = 0x%X\n",
             cmd.buffer.block_data[1]);
      printk(KERN_DEBUG "System_system control = 0x%X\n",
             cmd.buffer.block_data[2]);

      /* Bank 1 (VC) */
      cmd.req_mode = CAMERAACCESS_TYPE_BLOCK | CAMERAACCESS_VC;
      cmd.reg_count = 4;
      cmd.start = 0x80;
      cpia2_send_command(cam, &cmd);
      printk(KERN_DEBUG "ASIC_ID       = 0x%X\n",
             cmd.buffer.block_data[0]);
      printk(KERN_DEBUG "ASIC_REV      = 0x%X\n",
             cmd.buffer.block_data[1]);
      printk(KERN_DEBUG "PW_CONTRL     = 0x%X\n",
             cmd.buffer.block_data[2]);
      printk(KERN_DEBUG "WAKEUP        = 0x%X\n",
             cmd.buffer.block_data[3]);

      cmd.start = 0xA0; /* ST_CTRL */
      cmd.reg_count = 1;
      cpia2_send_command(cam, &cmd);
      printk(KERN_DEBUG "Stream ctrl   = 0x%X\n",
             cmd.buffer.block_data[0]);

      cmd.start = 0xA4; /* Stream status */
      cpia2_send_command(cam, &cmd);
      printk(KERN_DEBUG "Stream status = 0x%X\n",
             cmd.buffer.block_data[0]);

      cmd.start = 0xA8; /* USB status */
      cmd.reg_count = 3;
      cpia2_send_command(cam, &cmd);
      printk(KERN_DEBUG "USB_CTRL      = 0x%X\n",
             cmd.buffer.block_data[0]);
      printk(KERN_DEBUG "USB_STRM      = 0x%X\n",
             cmd.buffer.block_data[1]);
      printk(KERN_DEBUG "USB_STATUS    = 0x%X\n",
             cmd.buffer.block_data[2]);

      cmd.start = 0xAF; /* USB settings */
      cmd.reg_count = 1;
      cpia2_send_command(cam, &cmd);
      printk(KERN_DEBUG "USB settings  = 0x%X\n",
             cmd.buffer.block_data[0]);

      cmd.start = 0xC0; /* VC stuff */
      cmd.reg_count = 26;
      cpia2_send_command(cam, &cmd);
      printk(KERN_DEBUG "VC Control    = 0x%0X\n",
             cmd.buffer.block_data[0]);
      printk(KERN_DEBUG "VC Format     = 0x%0X\n",
             cmd.buffer.block_data[3]);
      printk(KERN_DEBUG "VC Clocks     = 0x%0X\n",
             cmd.buffer.block_data[4]);
      printk(KERN_DEBUG "VC IHSize     = 0x%0X\n",
             cmd.buffer.block_data[5]);
      printk(KERN_DEBUG "VC Xlim Hi    = 0x%0X\n",
             cmd.buffer.block_data[6]);
      printk(KERN_DEBUG "VC XLim Lo    = 0x%0X\n",
             cmd.buffer.block_data[7]);
      printk(KERN_DEBUG "VC YLim Hi    = 0x%0X\n",
             cmd.buffer.block_data[8]);
      printk(KERN_DEBUG "VC YLim Lo    = 0x%0X\n",
             cmd.buffer.block_data[9]);
      printk(KERN_DEBUG "VC OHSize     = 0x%0X\n",
             cmd.buffer.block_data[10]);
      printk(KERN_DEBUG "VC OVSize     = 0x%0X\n",
             cmd.buffer.block_data[11]);
      printk(KERN_DEBUG "VC HCrop      = 0x%0X\n",
             cmd.buffer.block_data[12]);
      printk(KERN_DEBUG "VC VCrop      = 0x%0X\n",
             cmd.buffer.block_data[13]);
      printk(KERN_DEBUG "VC HPhase     = 0x%0X\n",
             cmd.buffer.block_data[14]);
      printk(KERN_DEBUG "VC VPhase     = 0x%0X\n",
             cmd.buffer.block_data[15]);
      printk(KERN_DEBUG "VC HIspan     = 0x%0X\n",
             cmd.buffer.block_data[16]);
      printk(KERN_DEBUG "VC VIspan     = 0x%0X\n",
             cmd.buffer.block_data[17]);
      printk(KERN_DEBUG "VC HiCrop     = 0x%0X\n",
             cmd.buffer.block_data[18]);
      printk(KERN_DEBUG "VC ViCrop     = 0x%0X\n",
             cmd.buffer.block_data[19]);
      printk(KERN_DEBUG "VC HiFract    = 0x%0X\n",
             cmd.buffer.block_data[20]);
      printk(KERN_DEBUG "VC ViFract    = 0x%0X\n",
             cmd.buffer.block_data[21]);
      printk(KERN_DEBUG "VC JPeg Opt   = 0x%0X\n",
             cmd.buffer.block_data[22]);
      printk(KERN_DEBUG "VC Creep Per  = 0x%0X\n",
             cmd.buffer.block_data[23]);
      printk(KERN_DEBUG "VC User Sq.   = 0x%0X\n",
             cmd.buffer.block_data[24]);
      printk(KERN_DEBUG "VC Target KB  = 0x%0X\n",
             cmd.buffer.block_data[25]);

      /*** VP ***/
      cmd.req_mode = CAMERAACCESS_TYPE_BLOCK | CAMERAACCESS_VP;
      cmd.reg_count = 14;
      cmd.start = 0;
      cpia2_send_command(cam, &cmd);

      printk(KERN_DEBUG "VP Dev Hi     = 0x%0X\n",
             cmd.buffer.block_data[0]);
      printk(KERN_DEBUG "VP Dev Lo     = 0x%0X\n",
             cmd.buffer.block_data[1]);
      printk(KERN_DEBUG "VP Sys State  = 0x%0X\n",
             cmd.buffer.block_data[2]);
      printk(KERN_DEBUG "VP Sys Ctrl   = 0x%0X\n",
             cmd.buffer.block_data[3]);
      printk(KERN_DEBUG "VP Sensor flg = 0x%0X\n",
             cmd.buffer.block_data[5]);
      printk(KERN_DEBUG "VP Sensor Rev = 0x%0X\n",
             cmd.buffer.block_data[6]);
      printk(KERN_DEBUG "VP Dev Config = 0x%0X\n",
             cmd.buffer.block_data[7]);
      printk(KERN_DEBUG "VP GPIO_DIR   = 0x%0X\n",
             cmd.buffer.block_data[8]);
      printk(KERN_DEBUG "VP GPIO_DATA  = 0x%0X\n",
             cmd.buffer.block_data[9]);
      printk(KERN_DEBUG "VP Ram ADDR H = 0x%0X\n",
             cmd.buffer.block_data[10]);
      printk(KERN_DEBUG "VP Ram ADDR L = 0x%0X\n",
             cmd.buffer.block_data[11]);
      printk(KERN_DEBUG "VP RAM Data   = 0x%0X\n",
             cmd.buffer.block_data[12]);
      printk(KERN_DEBUG "Do Call       = 0x%0X\n",
             cmd.buffer.block_data[13]);

      if (cam->params.pnp_id.device_type == DEVICE_STV_672) {
            cmd.reg_count = 9;
            cmd.start = 0x0E;
            cpia2_send_command(cam, &cmd);
            printk(KERN_DEBUG "VP Clock Ctrl = 0x%0X\n",
                   cmd.buffer.block_data[0]);
            printk(KERN_DEBUG "VP Patch Rev  = 0x%0X\n",
                   cmd.buffer.block_data[1]);
            printk(KERN_DEBUG "VP Vid Mode   = 0x%0X\n",
                   cmd.buffer.block_data[2]);
            printk(KERN_DEBUG "VP Framerate  = 0x%0X\n",
                   cmd.buffer.block_data[3]);
            printk(KERN_DEBUG "VP UserEffect = 0x%0X\n",
                   cmd.buffer.block_data[4]);
            printk(KERN_DEBUG "VP White Bal  = 0x%0X\n",
                   cmd.buffer.block_data[5]);
            printk(KERN_DEBUG "VP WB thresh  = 0x%0X\n",
                   cmd.buffer.block_data[6]);
            printk(KERN_DEBUG "VP Exp Modes  = 0x%0X\n",
                   cmd.buffer.block_data[7]);
            printk(KERN_DEBUG "VP Exp Target = 0x%0X\n",
                   cmd.buffer.block_data[8]);

            cmd.reg_count = 1;
            cmd.start = 0x1B;
            cpia2_send_command(cam, &cmd);
            printk(KERN_DEBUG "VP FlickerMds = 0x%0X\n",
                   cmd.buffer.block_data[0]);
      } else {
            cmd.reg_count = 8 ;
            cmd.start = 0x0E;
            cpia2_send_command(cam, &cmd);
            printk(KERN_DEBUG "VP Clock Ctrl = 0x%0X\n",
                   cmd.buffer.block_data[0]);
            printk(KERN_DEBUG "VP Patch Rev  = 0x%0X\n",
                   cmd.buffer.block_data[1]);
            printk(KERN_DEBUG "VP Vid Mode   = 0x%0X\n",
                   cmd.buffer.block_data[5]);
            printk(KERN_DEBUG "VP Framerate  = 0x%0X\n",
                   cmd.buffer.block_data[6]);
            printk(KERN_DEBUG "VP UserEffect = 0x%0X\n",
                   cmd.buffer.block_data[7]);

            cmd.reg_count = 1;
            cmd.start = CPIA2_VP5_EXPOSURE_TARGET;
            cpia2_send_command(cam, &cmd);
            printk(KERN_DEBUG "VP5 Exp Target= 0x%0X\n",
                   cmd.buffer.block_data[0]);

            cmd.reg_count = 4;
            cmd.start = 0x3A;
            cpia2_send_command(cam, &cmd);
            printk(KERN_DEBUG "VP5 MY Black  = 0x%0X\n",
                   cmd.buffer.block_data[0]);
            printk(KERN_DEBUG "VP5 MCY Range = 0x%0X\n",
                   cmd.buffer.block_data[1]);
            printk(KERN_DEBUG "VP5 MYCEILING = 0x%0X\n",
                   cmd.buffer.block_data[2]);
            printk(KERN_DEBUG "VP5 MCUV Sat  = 0x%0X\n",
                   cmd.buffer.block_data[3]);
      }
#endif
}

/******************************************************************************
 *
 *  reset_camera_struct
 *
 *  Sets all values to the defaults
 *****************************************************************************/
void reset_camera_struct(struct camera_data *cam)
{
      /***
       * The following parameter values are the defaults from the register map.
       ***/
      cam->params.color_params.brightness = DEFAULT_BRIGHTNESS;
      cam->params.color_params.contrast = DEFAULT_CONTRAST;
      cam->params.color_params.saturation = DEFAULT_SATURATION;
      cam->params.vp_params.lowlight_boost = 0;

      /* FlickerModes */
      cam->params.flicker_control.flicker_mode_req = NEVER_FLICKER;
      cam->params.flicker_control.mains_frequency = 60;

      /* jpeg params */
      cam->params.compression.jpeg_options = CPIA2_VC_VC_JPEG_OPT_DEFAULT;
      cam->params.compression.creep_period = 2;
      cam->params.compression.user_squeeze = 20;
      cam->params.compression.inhibit_htables = false;

      /* gpio params */
      cam->params.vp_params.gpio_direction = 0; /* write, the default safe mode */
      cam->params.vp_params.gpio_data = 0;

      /* Target kb params */
      cam->params.vc_params.target_kb = DEFAULT_TARGET_KB;

      /***
       * Set Sensor FPS as fast as possible.
       ***/
      if(cam->params.pnp_id.device_type == DEVICE_STV_672) {
            if(cam->params.version.sensor_flags == CPIA2_VP_SENSOR_FLAGS_500)
                  cam->params.vp_params.frame_rate = CPIA2_VP_FRAMERATE_15;
            else
                  cam->params.vp_params.frame_rate = CPIA2_VP_FRAMERATE_30;
      } else {
            cam->params.vp_params.frame_rate = CPIA2_VP_FRAMERATE_30;
      }

      /***
       * Set default video mode as large as possible :
       * for vga sensor set to vga, for cif sensor set to CIF.
       ***/
      if (cam->params.version.sensor_flags == CPIA2_VP_SENSOR_FLAGS_500) {
            cam->sensor_type = CPIA2_SENSOR_500;
            cam->video_size = VIDEOSIZE_VGA;
            cam->params.roi.width = STV_IMAGE_VGA_COLS;
            cam->params.roi.height = STV_IMAGE_VGA_ROWS;
      } else {
            cam->sensor_type = CPIA2_SENSOR_410;
            cam->video_size = VIDEOSIZE_CIF;
            cam->params.roi.width = STV_IMAGE_CIF_COLS;
            cam->params.roi.height = STV_IMAGE_CIF_ROWS;
      }

      /***
       * Fill in the v4l structures.  video_cap is filled in inside the VIDIOCCAP
       * Ioctl.  Here, just do the window and picture stucts.
       ***/
      cam->vp.palette = (u16) VIDEO_PALETTE_RGB24;    /* Is this right? */
      cam->vp.brightness = (u16) cam->params.color_params.brightness * 256;
      cam->vp.colour = (u16) cam->params.color_params.saturation * 256;
      cam->vp.contrast = (u16) cam->params.color_params.contrast * 256;

      cam->vw.x = 0;
      cam->vw.y = 0;
      cam->vw.width = cam->params.roi.width;
      cam->vw.height = cam->params.roi.height;
      cam->vw.flags = 0;
      cam->vw.clipcount = 0;

      return;
}

/******************************************************************************
 *
 *  cpia2_init_camera_struct
 *
 *  Initializes camera struct, does not call reset to fill in defaults.
 *****************************************************************************/
struct camera_data *cpia2_init_camera_struct(void)
{
      struct camera_data *cam;

      cam = kzalloc(sizeof(*cam), GFP_KERNEL);

      if (!cam) {
            ERR("couldn't kmalloc cpia2 struct\n");
            return NULL;
      }


      cam->present = 1;
      mutex_init(&cam->busy_lock);
      init_waitqueue_head(&cam->wq_stream);

      return cam;
}

/******************************************************************************
 *
 *  cpia2_init_camera
 *
 *  Initializes camera.
 *****************************************************************************/
int cpia2_init_camera(struct camera_data *cam)
{
      DBG("Start\n");

      cam->mmapped = false;

      /* Get sensor and asic types before reset. */
      cpia2_set_high_power(cam);
      cpia2_get_version_info(cam);
      if (cam->params.version.asic_id != CPIA2_ASIC_672) {
            ERR("Device IO error (asicID has incorrect value of 0x%X\n",
                cam->params.version.asic_id);
            return -ENODEV;
      }

      /* Set GPIO direction and data to a safe state. */
      cpia2_do_command(cam, CPIA2_CMD_SET_VC_MP_GPIO_DIRECTION,
                   TRANSFER_WRITE, 0);
      cpia2_do_command(cam, CPIA2_CMD_SET_VC_MP_GPIO_DATA,
                   TRANSFER_WRITE, 0);

      /* resetting struct requires version info for sensor and asic types */
      reset_camera_struct(cam);

      cpia2_set_low_power(cam);

      DBG("End\n");

      return 0;
}

/******************************************************************************
 *
 *  cpia2_allocate_buffers
 *
 *****************************************************************************/
int cpia2_allocate_buffers(struct camera_data *cam)
{
      int i;

      if(!cam->buffers) {
            u32 size = cam->num_frames*sizeof(struct framebuf);
            cam->buffers = kmalloc(size, GFP_KERNEL);
            if(!cam->buffers) {
                  ERR("couldn't kmalloc frame buffer structures\n");
                  return -ENOMEM;
            }
      }

      if(!cam->frame_buffer) {
            cam->frame_buffer = rvmalloc(cam->frame_size*cam->num_frames);
            if (!cam->frame_buffer) {
                  ERR("couldn't vmalloc frame buffer data area\n");
                  kfree(cam->buffers);
                  cam->buffers = NULL;
                  return -ENOMEM;
            }
      }

      for(i=0; i<cam->num_frames-1; ++i) {
            cam->buffers[i].next = &cam->buffers[i+1];
            cam->buffers[i].data = cam->frame_buffer +i*cam->frame_size;
            cam->buffers[i].status = FRAME_EMPTY;
            cam->buffers[i].length = 0;
            cam->buffers[i].max_length = 0;
            cam->buffers[i].num = i;
      }
      cam->buffers[i].next = cam->buffers;
      cam->buffers[i].data = cam->frame_buffer +i*cam->frame_size;
      cam->buffers[i].status = FRAME_EMPTY;
      cam->buffers[i].length = 0;
      cam->buffers[i].max_length = 0;
      cam->buffers[i].num = i;
      cam->curbuff = cam->buffers;
      cam->workbuff = cam->curbuff->next;
      DBG("buffers=%p, curbuff=%p, workbuff=%p\n", cam->buffers, cam->curbuff,
          cam->workbuff);
      return 0;
}

/******************************************************************************
 *
 *  cpia2_free_buffers
 *
 *****************************************************************************/
void cpia2_free_buffers(struct camera_data *cam)
{
      if(cam->buffers) {
            kfree(cam->buffers);
            cam->buffers = NULL;
      }
      if(cam->frame_buffer) {
            rvfree(cam->frame_buffer, cam->frame_size*cam->num_frames);
            cam->frame_buffer = NULL;
      }
}

/******************************************************************************
 *
 *  cpia2_read
 *
 *****************************************************************************/
long cpia2_read(struct camera_data *cam,
            char __user *buf, unsigned long count, int noblock)
{
      struct framebuf *frame;
      if (!count) {
            return 0;
      }

      if (!buf) {
            ERR("%s: buffer NULL\n",__FUNCTION__);
            return -EINVAL;
      }

      if (!cam) {
            ERR("%s: Internal error, camera_data NULL!\n",__FUNCTION__);
            return -EINVAL;
      }

      /* make this _really_ smp and multithread-safe */
      if (mutex_lock_interruptible(&cam->busy_lock))
            return -ERESTARTSYS;

      if (!cam->present) {
            LOG("%s: camera removed\n",__FUNCTION__);
            mutex_unlock(&cam->busy_lock);
            return 0;   /* EOF */
      }

      if(!cam->streaming) {
            /* Start streaming */
            cpia2_usb_stream_start(cam,
                               cam->params.camera_state.stream_mode);
      }

      /* Copy cam->curbuff in case it changes while we're processing */
      frame = cam->curbuff;
      if (noblock && frame->status != FRAME_READY) {
            mutex_unlock(&cam->busy_lock);
            return -EAGAIN;
      }

      if(frame->status != FRAME_READY) {
            mutex_unlock(&cam->busy_lock);
            wait_event_interruptible(cam->wq_stream,
                         !cam->present ||
                         (frame = cam->curbuff)->status == FRAME_READY);
            if (signal_pending(current))
                  return -ERESTARTSYS;
            /* make this _really_ smp and multithread-safe */
            if (mutex_lock_interruptible(&cam->busy_lock)) {
                  return -ERESTARTSYS;
            }
            if(!cam->present) {
                  mutex_unlock(&cam->busy_lock);
                  return 0;
            }
      }

      /* copy data to user space */
      if (frame->length > count) {
            mutex_unlock(&cam->busy_lock);
            return -EFAULT;
      }
      if (copy_to_user(buf, frame->data, frame->length)) {
            mutex_unlock(&cam->busy_lock);
            return -EFAULT;
      }

      count = frame->length;

      frame->status = FRAME_EMPTY;

      mutex_unlock(&cam->busy_lock);
      return count;
}

/******************************************************************************
 *
 *  cpia2_poll
 *
 *****************************************************************************/
unsigned int cpia2_poll(struct camera_data *cam, struct file *filp,
                  poll_table *wait)
{
      unsigned int status=0;

      if(!cam) {
            ERR("%s: Internal error, camera_data not found!\n",__FUNCTION__);
            return POLLERR;
      }

      mutex_lock(&cam->busy_lock);

      if(!cam->present) {
            mutex_unlock(&cam->busy_lock);
            return POLLHUP;
      }

      if(!cam->streaming) {
            /* Start streaming */
            cpia2_usb_stream_start(cam,
                               cam->params.camera_state.stream_mode);
      }

      mutex_unlock(&cam->busy_lock);
      poll_wait(filp, &cam->wq_stream, wait);
      mutex_lock(&cam->busy_lock);

      if(!cam->present)
            status = POLLHUP;
      else if(cam->curbuff->status == FRAME_READY)
            status = POLLIN | POLLRDNORM;

      mutex_unlock(&cam->busy_lock);
      return status;
}

/******************************************************************************
 *
 *  cpia2_remap_buffer
 *
 *****************************************************************************/
int cpia2_remap_buffer(struct camera_data *cam, struct vm_area_struct *vma)
{
      const char *adr = (const char *)vma->vm_start;
      unsigned long size = vma->vm_end-vma->vm_start;
      unsigned long start_offset = vma->vm_pgoff << PAGE_SHIFT;
      unsigned long start = (unsigned long) adr;
      unsigned long page, pos;

      if (!cam)
            return -ENODEV;

      DBG("mmap offset:%ld size:%ld\n", start_offset, size);

      /* make this _really_ smp-safe */
      if (mutex_lock_interruptible(&cam->busy_lock))
            return -ERESTARTSYS;

      if (!cam->present) {
            mutex_unlock(&cam->busy_lock);
            return -ENODEV;
      }

      if (size > cam->frame_size*cam->num_frames  ||
          (start_offset % cam->frame_size) != 0 ||
          (start_offset+size > cam->frame_size*cam->num_frames)) {
            mutex_unlock(&cam->busy_lock);
            return -EINVAL;
      }

      pos = ((unsigned long) (cam->frame_buffer)) + start_offset;
      while (size > 0) {
            page = kvirt_to_pa(pos);
            if (remap_pfn_range(vma, start, page >> PAGE_SHIFT, PAGE_SIZE, PAGE_SHARED)) {
                  mutex_unlock(&cam->busy_lock);
                  return -EAGAIN;
            }
            start += PAGE_SIZE;
            pos += PAGE_SIZE;
            if (size > PAGE_SIZE)
                  size -= PAGE_SIZE;
            else
                  size = 0;
      }

      cam->mmapped = true;
      mutex_unlock(&cam->busy_lock);
      return 0;
}


Generated by  Doxygen 1.6.0   Back to index