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