drivers/media/usb/cpia2/cpia2_core.c

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