drivers/media/usb/cpia2/cpia2_core.c

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