drivers/media/video/cpia2/cpia2_core.c

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