drivers/media/video/cpia2/cpia2_core.c

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