drivers/media/video/cpia2/cpia2_core.c

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