git.oblomov.eu Git - linux-2.6/blob - drivers/media/video/cpia2/cpia2_core.c

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