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