drivers/media/usb/gspca/cpia1.c

   1 /*
   2  * cpia CPiA (1) gspca driver
   3  *
   4  * Copyright (C) 2010-2011 Hans de Goede <hdegoede@redhat.com>
   5  *
   6  * This module is adapted from the in kernel v4l1 cpia driver which is :
   7  *
   8  * (C) Copyright 1999-2000 Peter Pregler
   9  * (C) Copyright 1999-2000 Scott J. Bertin
  10  * (C) Copyright 1999-2000 Johannes Erdfelt <johannes@erdfelt.com>
  11  * (C) Copyright 2000 STMicroelectronics
  12  *
  13  * This program is free software; you can redistribute it and/or modify
  14  * it under the terms of the GNU General Public License as published by
  15  * the Free Software Foundation; either version 2 of the License, or
  16  * (at your option) any later version.
  17  *
  18  * This program is distributed in the hope that it will be useful,
  19  * but WITHOUT ANY WARRANTY; without even the implied warranty of
  20  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
  21  * GNU General Public License for more details.
  22  *
  23  */
  24
  25 #define pr_fmt(fmt) KBUILD_MODNAME ": " fmt
  26
  27 #define MODULE_NAME "cpia1"
  28
  29 #include <linux/input.h>
  30 #include <linux/sched/signal.h>
  31 #include <linux/bitops.h>
  32
  33 #include "gspca.h"
  34
  35 MODULE_AUTHOR("Hans de Goede <hdegoede@redhat.com>");
  36 MODULE_DESCRIPTION("Vision CPiA");
  37 MODULE_LICENSE("GPL");
  38
  39 /* constant value's */
  40 #define MAGIC_0         0x19
  41 #define MAGIC_1         0x68
  42 #define DATA_IN         0xc0
  43 #define DATA_OUT        0x40
  44 #define VIDEOSIZE_QCIF  0       /* 176x144 */
  45 #define VIDEOSIZE_CIF   1       /* 352x288 */
  46 #define SUBSAMPLE_420   0
  47 #define SUBSAMPLE_422   1
  48 #define YUVORDER_YUYV   0
  49 #define YUVORDER_UYVY   1
  50 #define NOT_COMPRESSED  0
  51 #define COMPRESSED      1
  52 #define NO_DECIMATION   0
  53 #define DECIMATION_ENAB 1
  54 #define EOI             0xff    /* End Of Image */
  55 #define EOL             0xfd    /* End Of Line */
  56 #define FRAME_HEADER_SIZE       64
  57
  58 /* Image grab modes */
  59 #define CPIA_GRAB_SINGLE        0
  60 #define CPIA_GRAB_CONTINEOUS    1
  61
  62 /* Compression parameters */
  63 #define CPIA_COMPRESSION_NONE   0
  64 #define CPIA_COMPRESSION_AUTO   1
  65 #define CPIA_COMPRESSION_MANUAL 2
  66 #define CPIA_COMPRESSION_TARGET_QUALITY         0
  67 #define CPIA_COMPRESSION_TARGET_FRAMERATE       1
  68
  69 /* Return offsets for GetCameraState */
  70 #define SYSTEMSTATE     0
  71 #define GRABSTATE       1
  72 #define STREAMSTATE     2
  73 #define FATALERROR      3
  74 #define CMDERROR        4
  75 #define DEBUGFLAGS      5
  76 #define VPSTATUS        6
  77 #define ERRORCODE       7
  78
  79 /* SystemState */
  80 #define UNINITIALISED_STATE     0
  81 #define PASS_THROUGH_STATE      1
  82 #define LO_POWER_STATE          2
  83 #define HI_POWER_STATE          3
  84 #define WARM_BOOT_STATE         4
  85
  86 /* GrabState */
  87 #define GRAB_IDLE               0
  88 #define GRAB_ACTIVE             1
  89 #define GRAB_DONE               2
  90
  91 /* StreamState */
  92 #define STREAM_NOT_READY        0
  93 #define STREAM_READY            1
  94 #define STREAM_OPEN             2
  95 #define STREAM_PAUSED           3
  96 #define STREAM_FINISHED         4
  97
  98 /* Fatal Error, CmdError, and DebugFlags */
  99 #define CPIA_FLAG         1
 100 #define SYSTEM_FLAG       2
 101 #define INT_CTRL_FLAG     4
 102 #define PROCESS_FLAG      8
 103 #define COM_FLAG         16
 104 #define VP_CTRL_FLAG     32
 105 #define CAPTURE_FLAG     64
 106 #define DEBUG_FLAG      128
 107
 108 /* VPStatus */
 109 #define VP_STATE_OK                     0x00
 110
 111 #define VP_STATE_FAILED_VIDEOINIT       0x01
 112 #define VP_STATE_FAILED_AECACBINIT      0x02
 113 #define VP_STATE_AEC_MAX                0x04
 114 #define VP_STATE_ACB_BMAX               0x08
 115
 116 #define VP_STATE_ACB_RMIN               0x10
 117 #define VP_STATE_ACB_GMIN               0x20
 118 #define VP_STATE_ACB_RMAX               0x40
 119 #define VP_STATE_ACB_GMAX               0x80
 120
 121 /* default (minimum) compensation values */
 122 #define COMP_RED        220
 123 #define COMP_GREEN1     214
 124 #define COMP_GREEN2     COMP_GREEN1
 125 #define COMP_BLUE       230
 126
 127 /* exposure status */
 128 #define EXPOSURE_VERY_LIGHT 0
 129 #define EXPOSURE_LIGHT      1
 130 #define EXPOSURE_NORMAL     2
 131 #define EXPOSURE_DARK       3
 132 #define EXPOSURE_VERY_DARK  4
 133
 134 #define CPIA_MODULE_CPIA                        (0 << 5)
 135 #define CPIA_MODULE_SYSTEM                      (1 << 5)
 136 #define CPIA_MODULE_VP_CTRL                     (5 << 5)
 137 #define CPIA_MODULE_CAPTURE                     (6 << 5)
 138 #define CPIA_MODULE_DEBUG                       (7 << 5)
 139
 140 #define INPUT (DATA_IN << 8)
 141 #define OUTPUT (DATA_OUT << 8)
 142
 143 #define CPIA_COMMAND_GetCPIAVersion     (INPUT | CPIA_MODULE_CPIA | 1)
 144 #define CPIA_COMMAND_GetPnPID           (INPUT | CPIA_MODULE_CPIA | 2)
 145 #define CPIA_COMMAND_GetCameraStatus    (INPUT | CPIA_MODULE_CPIA | 3)
 146 #define CPIA_COMMAND_GotoHiPower        (OUTPUT | CPIA_MODULE_CPIA | 4)
 147 #define CPIA_COMMAND_GotoLoPower        (OUTPUT | CPIA_MODULE_CPIA | 5)
 148 #define CPIA_COMMAND_GotoSuspend        (OUTPUT | CPIA_MODULE_CPIA | 7)
 149 #define CPIA_COMMAND_GotoPassThrough    (OUTPUT | CPIA_MODULE_CPIA | 8)
 150 #define CPIA_COMMAND_ModifyCameraStatus (OUTPUT | CPIA_MODULE_CPIA | 10)
 151
 152 #define CPIA_COMMAND_ReadVCRegs         (INPUT | CPIA_MODULE_SYSTEM | 1)
 153 #define CPIA_COMMAND_WriteVCReg         (OUTPUT | CPIA_MODULE_SYSTEM | 2)
 154 #define CPIA_COMMAND_ReadMCPorts        (INPUT | CPIA_MODULE_SYSTEM | 3)
 155 #define CPIA_COMMAND_WriteMCPort        (OUTPUT | CPIA_MODULE_SYSTEM | 4)
 156 #define CPIA_COMMAND_SetBaudRate        (OUTPUT | CPIA_MODULE_SYSTEM | 5)
 157 #define CPIA_COMMAND_SetECPTiming       (OUTPUT | CPIA_MODULE_SYSTEM | 6)
 158 #define CPIA_COMMAND_ReadIDATA          (INPUT | CPIA_MODULE_SYSTEM | 7)
 159 #define CPIA_COMMAND_WriteIDATA         (OUTPUT | CPIA_MODULE_SYSTEM | 8)
 160 #define CPIA_COMMAND_GenericCall        (OUTPUT | CPIA_MODULE_SYSTEM | 9)
 161 #define CPIA_COMMAND_I2CStart           (OUTPUT | CPIA_MODULE_SYSTEM | 10)
 162 #define CPIA_COMMAND_I2CStop            (OUTPUT | CPIA_MODULE_SYSTEM | 11)
 163 #define CPIA_COMMAND_I2CWrite           (OUTPUT | CPIA_MODULE_SYSTEM | 12)
 164 #define CPIA_COMMAND_I2CRead            (INPUT | CPIA_MODULE_SYSTEM | 13)
 165
 166 #define CPIA_COMMAND_GetVPVersion       (INPUT | CPIA_MODULE_VP_CTRL | 1)
 167 #define CPIA_COMMAND_ResetFrameCounter  (INPUT | CPIA_MODULE_VP_CTRL | 2)
 168 #define CPIA_COMMAND_SetColourParams    (OUTPUT | CPIA_MODULE_VP_CTRL | 3)
 169 #define CPIA_COMMAND_SetExposure        (OUTPUT | CPIA_MODULE_VP_CTRL | 4)
 170 #define CPIA_COMMAND_SetColourBalance   (OUTPUT | CPIA_MODULE_VP_CTRL | 6)
 171 #define CPIA_COMMAND_SetSensorFPS       (OUTPUT | CPIA_MODULE_VP_CTRL | 7)
 172 #define CPIA_COMMAND_SetVPDefaults      (OUTPUT | CPIA_MODULE_VP_CTRL | 8)
 173 #define CPIA_COMMAND_SetApcor           (OUTPUT | CPIA_MODULE_VP_CTRL | 9)
 174 #define CPIA_COMMAND_SetFlickerCtrl     (OUTPUT | CPIA_MODULE_VP_CTRL | 10)
 175 #define CPIA_COMMAND_SetVLOffset        (OUTPUT | CPIA_MODULE_VP_CTRL | 11)
 176 #define CPIA_COMMAND_GetColourParams    (INPUT | CPIA_MODULE_VP_CTRL | 16)
 177 #define CPIA_COMMAND_GetColourBalance   (INPUT | CPIA_MODULE_VP_CTRL | 17)
 178 #define CPIA_COMMAND_GetExposure        (INPUT | CPIA_MODULE_VP_CTRL | 18)
 179 #define CPIA_COMMAND_SetSensorMatrix    (OUTPUT | CPIA_MODULE_VP_CTRL | 19)
 180 #define CPIA_COMMAND_ColourBars         (OUTPUT | CPIA_MODULE_VP_CTRL | 25)
 181 #define CPIA_COMMAND_ReadVPRegs         (INPUT | CPIA_MODULE_VP_CTRL | 30)
 182 #define CPIA_COMMAND_WriteVPReg         (OUTPUT | CPIA_MODULE_VP_CTRL | 31)
 183
 184 #define CPIA_COMMAND_GrabFrame          (OUTPUT | CPIA_MODULE_CAPTURE | 1)
 185 #define CPIA_COMMAND_UploadFrame        (OUTPUT | CPIA_MODULE_CAPTURE | 2)
 186 #define CPIA_COMMAND_SetGrabMode        (OUTPUT | CPIA_MODULE_CAPTURE | 3)
 187 #define CPIA_COMMAND_InitStreamCap      (OUTPUT | CPIA_MODULE_CAPTURE | 4)
 188 #define CPIA_COMMAND_FiniStreamCap      (OUTPUT | CPIA_MODULE_CAPTURE | 5)
 189 #define CPIA_COMMAND_StartStreamCap     (OUTPUT | CPIA_MODULE_CAPTURE | 6)
 190 #define CPIA_COMMAND_EndStreamCap       (OUTPUT | CPIA_MODULE_CAPTURE | 7)
 191 #define CPIA_COMMAND_SetFormat          (OUTPUT | CPIA_MODULE_CAPTURE | 8)
 192 #define CPIA_COMMAND_SetROI             (OUTPUT | CPIA_MODULE_CAPTURE | 9)
 193 #define CPIA_COMMAND_SetCompression     (OUTPUT | CPIA_MODULE_CAPTURE | 10)
 194 #define CPIA_COMMAND_SetCompressionTarget (OUTPUT | CPIA_MODULE_CAPTURE | 11)
 195 #define CPIA_COMMAND_SetYUVThresh       (OUTPUT | CPIA_MODULE_CAPTURE | 12)
 196 #define CPIA_COMMAND_SetCompressionParams (OUTPUT | CPIA_MODULE_CAPTURE | 13)
 197 #define CPIA_COMMAND_DiscardFrame       (OUTPUT | CPIA_MODULE_CAPTURE | 14)
 198 #define CPIA_COMMAND_GrabReset          (OUTPUT | CPIA_MODULE_CAPTURE | 15)
 199
 200 #define CPIA_COMMAND_OutputRS232        (OUTPUT | CPIA_MODULE_DEBUG | 1)
 201 #define CPIA_COMMAND_AbortProcess       (OUTPUT | CPIA_MODULE_DEBUG | 4)
 202 #define CPIA_COMMAND_SetDramPage        (OUTPUT | CPIA_MODULE_DEBUG | 5)
 203 #define CPIA_COMMAND_StartDramUpload    (OUTPUT | CPIA_MODULE_DEBUG | 6)
 204 #define CPIA_COMMAND_StartDummyDtream   (OUTPUT | CPIA_MODULE_DEBUG | 8)
 205 #define CPIA_COMMAND_AbortStream        (OUTPUT | CPIA_MODULE_DEBUG | 9)
 206 #define CPIA_COMMAND_DownloadDRAM       (OUTPUT | CPIA_MODULE_DEBUG | 10)
 207 #define CPIA_COMMAND_Null               (OUTPUT | CPIA_MODULE_DEBUG | 11)
 208
 209 #define ROUND_UP_EXP_FOR_FLICKER 15
 210
 211 /* Constants for automatic frame rate adjustment */
 212 #define MAX_EXP       302
 213 #define MAX_EXP_102   255
 214 #define LOW_EXP       140
 215 #define VERY_LOW_EXP   70
 216 #define TC             94
 217 #define EXP_ACC_DARK   50
 218 #define EXP_ACC_LIGHT  90
 219 #define HIGH_COMP_102 160
 220 #define MAX_COMP      239
 221 #define DARK_TIME       3
 222 #define LIGHT_TIME      3
 223
 224 #define FIRMWARE_VERSION(x, y) (sd->params.version.firmwareVersion == (x) && \
 225                                 sd->params.version.firmwareRevision == (y))
 226
 227 #define CPIA1_CID_COMP_TARGET (V4L2_CTRL_CLASS_USER + 0x1000)
 228 #define BRIGHTNESS_DEF 50
 229 #define CONTRAST_DEF 48
 230 #define SATURATION_DEF 50
 231 #define FREQ_DEF V4L2_CID_POWER_LINE_FREQUENCY_50HZ
 232 #define ILLUMINATORS_1_DEF 0
 233 #define ILLUMINATORS_2_DEF 0
 234 #define COMP_TARGET_DEF CPIA_COMPRESSION_TARGET_QUALITY
 235
 236 /* Developer's Guide Table 5 p 3-34
 237  * indexed by [mains][sensorFps.baserate][sensorFps.divisor]*/
 238 static u8 flicker_jumps[2][2][4] =
 239 { { { 76, 38, 19, 9 }, { 92, 46, 23, 11 } },
 240   { { 64, 32, 16, 8 }, { 76, 38, 19, 9} }
 241 };
 242
 243 struct cam_params {
 244         struct {
 245                 u8 firmwareVersion;
 246                 u8 firmwareRevision;
 247                 u8 vcVersion;
 248                 u8 vcRevision;
 249         } version;
 250         struct {
 251                 u16 vendor;
 252                 u16 product;
 253                 u16 deviceRevision;
 254         } pnpID;
 255         struct {
 256                 u8 vpVersion;
 257                 u8 vpRevision;
 258                 u16 cameraHeadID;
 259         } vpVersion;
 260         struct {
 261                 u8 systemState;
 262                 u8 grabState;
 263                 u8 streamState;
 264                 u8 fatalError;
 265                 u8 cmdError;
 266                 u8 debugFlags;
 267                 u8 vpStatus;
 268                 u8 errorCode;
 269         } status;
 270         struct {
 271                 u8 brightness;
 272                 u8 contrast;
 273                 u8 saturation;
 274         } colourParams;
 275         struct {
 276                 u8 gainMode;
 277                 u8 expMode;
 278                 u8 compMode;
 279                 u8 centreWeight;
 280                 u8 gain;
 281                 u8 fineExp;
 282                 u8 coarseExpLo;
 283                 u8 coarseExpHi;
 284                 u8 redComp;
 285                 u8 green1Comp;
 286                 u8 green2Comp;
 287                 u8 blueComp;
 288         } exposure;
 289         struct {
 290                 u8 balanceMode;
 291                 u8 redGain;
 292                 u8 greenGain;
 293                 u8 blueGain;
 294         } colourBalance;
 295         struct {
 296                 u8 divisor;
 297                 u8 baserate;
 298         } sensorFps;
 299         struct {
 300                 u8 gain1;
 301                 u8 gain2;
 302                 u8 gain4;
 303                 u8 gain8;
 304         } apcor;
 305         struct {
 306                 u8 disabled;
 307                 u8 flickerMode;
 308                 u8 coarseJump;
 309                 u8 allowableOverExposure;
 310         } flickerControl;
 311         struct {
 312                 u8 gain1;
 313                 u8 gain2;
 314                 u8 gain4;
 315                 u8 gain8;
 316         } vlOffset;
 317         struct {
 318                 u8 mode;
 319                 u8 decimation;
 320         } compression;
 321         struct {
 322                 u8 frTargeting;
 323                 u8 targetFR;
 324                 u8 targetQ;
 325         } compressionTarget;
 326         struct {
 327                 u8 yThreshold;
 328                 u8 uvThreshold;
 329         } yuvThreshold;
 330         struct {
 331                 u8 hysteresis;
 332                 u8 threshMax;
 333                 u8 smallStep;
 334                 u8 largeStep;
 335                 u8 decimationHysteresis;
 336                 u8 frDiffStepThresh;
 337                 u8 qDiffStepThresh;
 338                 u8 decimationThreshMod;
 339         } compressionParams;
 340         struct {
 341                 u8 videoSize;           /* CIF/QCIF */
 342                 u8 subSample;
 343                 u8 yuvOrder;
 344         } format;
 345         struct {                        /* Intel QX3 specific data */
 346                 u8 qx3_detected;        /* a QX3 is present */
 347                 u8 toplight;            /* top light lit , R/W */
 348                 u8 bottomlight;         /* bottom light lit, R/W */
 349                 u8 button;              /* snapshot button pressed (R/O) */
 350                 u8 cradled;             /* microscope is in cradle (R/O) */
 351         } qx3;
 352         struct {
 353                 u8 colStart;            /* skip first 8*colStart pixels */
 354                 u8 colEnd;              /* finish at 8*colEnd pixels */
 355                 u8 rowStart;            /* skip first 4*rowStart lines */
 356                 u8 rowEnd;              /* finish at 4*rowEnd lines */
 357         } roi;
 358         u8 ecpTiming;
 359         u8 streamStartLine;
 360 };
 361
 362 /* specific webcam descriptor */
 363 struct sd {
 364         struct gspca_dev gspca_dev;             /* !! must be the first item */
 365         struct cam_params params;               /* camera settings */
 366
 367         atomic_t cam_exposure;
 368         atomic_t fps;
 369         int exposure_count;
 370         u8 exposure_status;
 371         struct v4l2_ctrl *freq;
 372         u8 mainsFreq;                           /* 0 = 50hz, 1 = 60hz */
 373         u8 first_frame;
 374 };
 375
 376 static const struct v4l2_pix_format mode[] = {
 377         {160, 120, V4L2_PIX_FMT_CPIA1, V4L2_FIELD_NONE,
 378                 /* The sizeimage is trial and error, as with low framerates
 379                    the camera will pad out usb frames, making the image
 380                    data larger then strictly necessary */
 381                 .bytesperline = 160,
 382                 .sizeimage = 65536,
 383                 .colorspace = V4L2_COLORSPACE_SRGB,
 384                 .priv = 3},
 385         {176, 144, V4L2_PIX_FMT_CPIA1, V4L2_FIELD_NONE,
 386                 .bytesperline = 172,
 387                 .sizeimage = 65536,
 388                 .colorspace = V4L2_COLORSPACE_SRGB,
 389                 .priv = 2},
 390         {320, 240, V4L2_PIX_FMT_CPIA1, V4L2_FIELD_NONE,
 391                 .bytesperline = 320,
 392                 .sizeimage = 262144,
 393                 .colorspace = V4L2_COLORSPACE_SRGB,
 394                 .priv = 1},
 395         {352, 288, V4L2_PIX_FMT_CPIA1, V4L2_FIELD_NONE,
 396                 .bytesperline = 352,
 397                 .sizeimage = 262144,
 398                 .colorspace = V4L2_COLORSPACE_SRGB,
 399                 .priv = 0},
 400 };
 401
 402 /**********************************************************************
 403  *
 404  * General functions
 405  *
 406  **********************************************************************/
 407
 408 static int cpia_usb_transferCmd(struct gspca_dev *gspca_dev, u8 *command)
 409 {
 410         u8 requesttype;
 411         unsigned int pipe;
 412         int ret, databytes = command[6] | (command[7] << 8);
 413         /* Sometimes we see spurious EPIPE errors */
 414         int retries = 3;
 415
 416         if (command[0] == DATA_IN) {
 417                 pipe = usb_rcvctrlpipe(gspca_dev->dev, 0);
 418                 requesttype = USB_DIR_IN | USB_TYPE_VENDOR | USB_RECIP_DEVICE;
 419         } else if (command[0] == DATA_OUT) {
 420                 pipe = usb_sndctrlpipe(gspca_dev->dev, 0);
 421                 requesttype = USB_TYPE_VENDOR | USB_RECIP_DEVICE;
 422         } else {
 423                 gspca_err(gspca_dev, "Unexpected first byte of command: %x\n",
 424                           command[0]);
 425                 return -EINVAL;
 426         }
 427
 428 retry:
 429         ret = usb_control_msg(gspca_dev->dev, pipe,
 430                               command[1],
 431                               requesttype,
 432                               command[2] | (command[3] << 8),
 433                               command[4] | (command[5] << 8),
 434                               gspca_dev->usb_buf, databytes, 1000);
 435
 436         if (ret < 0)
 437                 pr_err("usb_control_msg %02x, error %d\n", command[1], ret);
 438
 439         if (ret == -EPIPE && retries > 0) {
 440                 retries--;
 441                 goto retry;
 442         }
 443
 444         return (ret < 0) ? ret : 0;
 445 }
 446
 447 /* send an arbitrary command to the camera */
 448 static int do_command(struct gspca_dev *gspca_dev, u16 command,
 449                       u8 a, u8 b, u8 c, u8 d)
 450 {
 451         struct sd *sd = (struct sd *) gspca_dev;
 452         int ret, datasize;
 453         u8 cmd[8];
 454
 455         switch (command) {
 456         case CPIA_COMMAND_GetCPIAVersion:
 457         case CPIA_COMMAND_GetPnPID:
 458         case CPIA_COMMAND_GetCameraStatus:
 459         case CPIA_COMMAND_GetVPVersion:
 460         case CPIA_COMMAND_GetColourParams:
 461         case CPIA_COMMAND_GetColourBalance:
 462         case CPIA_COMMAND_GetExposure:
 463                 datasize = 8;
 464                 break;
 465         case CPIA_COMMAND_ReadMCPorts:
 466         case CPIA_COMMAND_ReadVCRegs:
 467                 datasize = 4;
 468                 break;
 469         default:
 470                 datasize = 0;
 471                 break;
 472         }
 473
 474         cmd[0] = command >> 8;
 475         cmd[1] = command & 0xff;
 476         cmd[2] = a;
 477         cmd[3] = b;
 478         cmd[4] = c;
 479         cmd[5] = d;
 480         cmd[6] = datasize;
 481         cmd[7] = 0;
 482
 483         ret = cpia_usb_transferCmd(gspca_dev, cmd);
 484         if (ret)
 485                 return ret;
 486
 487         switch (command) {
 488         case CPIA_COMMAND_GetCPIAVersion:
 489                 sd->params.version.firmwareVersion = gspca_dev->usb_buf[0];
 490                 sd->params.version.firmwareRevision = gspca_dev->usb_buf[1];
 491                 sd->params.version.vcVersion = gspca_dev->usb_buf[2];
 492                 sd->params.version.vcRevision = gspca_dev->usb_buf[3];
 493                 break;
 494         case CPIA_COMMAND_GetPnPID:
 495                 sd->params.pnpID.vendor =
 496                         gspca_dev->usb_buf[0] | (gspca_dev->usb_buf[1] << 8);
 497                 sd->params.pnpID.product =
 498                         gspca_dev->usb_buf[2] | (gspca_dev->usb_buf[3] << 8);
 499                 sd->params.pnpID.deviceRevision =
 500                         gspca_dev->usb_buf[4] | (gspca_dev->usb_buf[5] << 8);
 501                 break;
 502         case CPIA_COMMAND_GetCameraStatus:
 503                 sd->params.status.systemState = gspca_dev->usb_buf[0];
 504                 sd->params.status.grabState = gspca_dev->usb_buf[1];
 505                 sd->params.status.streamState = gspca_dev->usb_buf[2];
 506                 sd->params.status.fatalError = gspca_dev->usb_buf[3];
 507                 sd->params.status.cmdError = gspca_dev->usb_buf[4];
 508                 sd->params.status.debugFlags = gspca_dev->usb_buf[5];
 509                 sd->params.status.vpStatus = gspca_dev->usb_buf[6];
 510                 sd->params.status.errorCode = gspca_dev->usb_buf[7];
 511                 break;
 512         case CPIA_COMMAND_GetVPVersion:
 513                 sd->params.vpVersion.vpVersion = gspca_dev->usb_buf[0];
 514                 sd->params.vpVersion.vpRevision = gspca_dev->usb_buf[1];
 515                 sd->params.vpVersion.cameraHeadID =
 516                         gspca_dev->usb_buf[2] | (gspca_dev->usb_buf[3] << 8);
 517                 break;
 518         case CPIA_COMMAND_GetColourParams:
 519                 sd->params.colourParams.brightness = gspca_dev->usb_buf[0];
 520                 sd->params.colourParams.contrast = gspca_dev->usb_buf[1];
 521                 sd->params.colourParams.saturation = gspca_dev->usb_buf[2];
 522                 break;
 523         case CPIA_COMMAND_GetColourBalance:
 524                 sd->params.colourBalance.redGain = gspca_dev->usb_buf[0];
 525                 sd->params.colourBalance.greenGain = gspca_dev->usb_buf[1];
 526                 sd->params.colourBalance.blueGain = gspca_dev->usb_buf[2];
 527                 break;
 528         case CPIA_COMMAND_GetExposure:
 529                 sd->params.exposure.gain = gspca_dev->usb_buf[0];
 530                 sd->params.exposure.fineExp = gspca_dev->usb_buf[1];
 531                 sd->params.exposure.coarseExpLo = gspca_dev->usb_buf[2];
 532                 sd->params.exposure.coarseExpHi = gspca_dev->usb_buf[3];
 533                 sd->params.exposure.redComp = gspca_dev->usb_buf[4];
 534                 sd->params.exposure.green1Comp = gspca_dev->usb_buf[5];
 535                 sd->params.exposure.green2Comp = gspca_dev->usb_buf[6];
 536                 sd->params.exposure.blueComp = gspca_dev->usb_buf[7];
 537                 break;
 538
 539         case CPIA_COMMAND_ReadMCPorts:
 540                 /* test button press */
 541                 a = ((gspca_dev->usb_buf[1] & 0x02) == 0);
 542                 if (a != sd->params.qx3.button) {
 543 #if IS_ENABLED(CONFIG_INPUT)
 544                         input_report_key(gspca_dev->input_dev, KEY_CAMERA, a);
 545                         input_sync(gspca_dev->input_dev);
 546 #endif
 547                         sd->params.qx3.button = a;
 548                 }
 549                 if (sd->params.qx3.button) {
 550                         /* button pressed - unlock the latch */
 551                         do_command(gspca_dev, CPIA_COMMAND_WriteMCPort,
 552                                    3, 0xdf, 0xdf, 0);
 553                         do_command(gspca_dev, CPIA_COMMAND_WriteMCPort,
 554                                    3, 0xff, 0xff, 0);
 555                 }
 556
 557                 /* test whether microscope is cradled */
 558                 sd->params.qx3.cradled = ((gspca_dev->usb_buf[2] & 0x40) == 0);
 559                 break;
 560         }
 561
 562         return 0;
 563 }
 564
 565 /* send a command to the camera with an additional data transaction */
 566 static int do_command_extended(struct gspca_dev *gspca_dev, u16 command,
 567                                u8 a, u8 b, u8 c, u8 d,
 568                                u8 e, u8 f, u8 g, u8 h,
 569                                u8 i, u8 j, u8 k, u8 l)
 570 {
 571         u8 cmd[8];
 572
 573         cmd[0] = command >> 8;
 574         cmd[1] = command & 0xff;
 575         cmd[2] = a;
 576         cmd[3] = b;
 577         cmd[4] = c;
 578         cmd[5] = d;
 579         cmd[6] = 8;
 580         cmd[7] = 0;
 581         gspca_dev->usb_buf[0] = e;
 582         gspca_dev->usb_buf[1] = f;
 583         gspca_dev->usb_buf[2] = g;
 584         gspca_dev->usb_buf[3] = h;
 585         gspca_dev->usb_buf[4] = i;
 586         gspca_dev->usb_buf[5] = j;
 587         gspca_dev->usb_buf[6] = k;
 588         gspca_dev->usb_buf[7] = l;
 589
 590         return cpia_usb_transferCmd(gspca_dev, cmd);
 591 }
 592
 593 /*  find_over_exposure
 594  *  Finds a suitable value of OverExposure for use with SetFlickerCtrl
 595  *  Some calculation is required because this value changes with the brightness
 596  *  set with SetColourParameters
 597  *
 598  *  Parameters: Brightness - last brightness value set with SetColourParameters
 599  *
 600  *  Returns: OverExposure value to use with SetFlickerCtrl
 601  */
 602 #define FLICKER_MAX_EXPOSURE                    250
 603 #define FLICKER_ALLOWABLE_OVER_EXPOSURE         146
 604 #define FLICKER_BRIGHTNESS_CONSTANT             59
 605 static int find_over_exposure(int brightness)
 606 {
 607         int MaxAllowableOverExposure, OverExposure;
 608
 609         MaxAllowableOverExposure = FLICKER_MAX_EXPOSURE - brightness -
 610                                    FLICKER_BRIGHTNESS_CONSTANT;
 611
 612         if (MaxAllowableOverExposure < FLICKER_ALLOWABLE_OVER_EXPOSURE)
 613                 OverExposure = MaxAllowableOverExposure;
 614         else
 615                 OverExposure = FLICKER_ALLOWABLE_OVER_EXPOSURE;
 616
 617         return OverExposure;
 618 }
 619 #undef FLICKER_MAX_EXPOSURE
 620 #undef FLICKER_ALLOWABLE_OVER_EXPOSURE
 621 #undef FLICKER_BRIGHTNESS_CONSTANT
 622
 623 /* initialise cam_data structure  */
 624 static void reset_camera_params(struct gspca_dev *gspca_dev)
 625 {
 626         struct sd *sd = (struct sd *) gspca_dev;
 627         struct cam_params *params = &sd->params;
 628
 629         /* The following parameter values are the defaults from
 630          * "Software Developer's Guide for CPiA Cameras".  Any changes
 631          * to the defaults are noted in comments. */
 632         params->colourParams.brightness = BRIGHTNESS_DEF;
 633         params->colourParams.contrast = CONTRAST_DEF;
 634         params->colourParams.saturation = SATURATION_DEF;
 635         params->exposure.gainMode = 4;
 636         params->exposure.expMode = 2;           /* AEC */
 637         params->exposure.compMode = 1;
 638         params->exposure.centreWeight = 1;
 639         params->exposure.gain = 0;
 640         params->exposure.fineExp = 0;
 641         params->exposure.coarseExpLo = 185;
 642         params->exposure.coarseExpHi = 0;
 643         params->exposure.redComp = COMP_RED;
 644         params->exposure.green1Comp = COMP_GREEN1;
 645         params->exposure.green2Comp = COMP_GREEN2;
 646         params->exposure.blueComp = COMP_BLUE;
 647         params->colourBalance.balanceMode = 2;  /* ACB */
 648         params->colourBalance.redGain = 32;
 649         params->colourBalance.greenGain = 6;
 650         params->colourBalance.blueGain = 92;
 651         params->apcor.gain1 = 0x18;
 652         params->apcor.gain2 = 0x16;
 653         params->apcor.gain4 = 0x24;
 654         params->apcor.gain8 = 0x34;
 655         params->vlOffset.gain1 = 20;
 656         params->vlOffset.gain2 = 24;
 657         params->vlOffset.gain4 = 26;
 658         params->vlOffset.gain8 = 26;
 659         params->compressionParams.hysteresis = 3;
 660         params->compressionParams.threshMax = 11;
 661         params->compressionParams.smallStep = 1;
 662         params->compressionParams.largeStep = 3;
 663         params->compressionParams.decimationHysteresis = 2;
 664         params->compressionParams.frDiffStepThresh = 5;
 665         params->compressionParams.qDiffStepThresh = 3;
 666         params->compressionParams.decimationThreshMod = 2;
 667         /* End of default values from Software Developer's Guide */
 668
 669         /* Set Sensor FPS to 15fps. This seems better than 30fps
 670          * for indoor lighting. */
 671         params->sensorFps.divisor = 1;
 672         params->sensorFps.baserate = 1;
 673
 674         params->flickerControl.flickerMode = 0;
 675         params->flickerControl.disabled = 1;
 676         params->flickerControl.coarseJump =
 677                 flicker_jumps[sd->mainsFreq]
 678                              [params->sensorFps.baserate]
 679                              [params->sensorFps.divisor];
 680         params->flickerControl.allowableOverExposure =
 681                 find_over_exposure(params->colourParams.brightness);
 682
 683         params->yuvThreshold.yThreshold = 6; /* From windows driver */
 684         params->yuvThreshold.uvThreshold = 6; /* From windows driver */
 685
 686         params->format.subSample = SUBSAMPLE_420;
 687         params->format.yuvOrder = YUVORDER_YUYV;
 688
 689         params->compression.mode = CPIA_COMPRESSION_AUTO;
 690         params->compression.decimation = NO_DECIMATION;
 691
 692         params->compressionTarget.frTargeting = COMP_TARGET_DEF;
 693         params->compressionTarget.targetFR = 15; /* From windows driver */
 694         params->compressionTarget.targetQ = 5; /* From windows driver */
 695
 696         params->qx3.qx3_detected = 0;
 697         params->qx3.toplight = 0;
 698         params->qx3.bottomlight = 0;
 699         params->qx3.button = 0;
 700         params->qx3.cradled = 0;
 701 }
 702
 703 static void printstatus(struct gspca_dev *gspca_dev, struct cam_params *params)
 704 {
 705         gspca_dbg(gspca_dev, D_PROBE, "status: %02x %02x %02x %02x %02x %02x %02x %02x\n",
 706                   params->status.systemState, params->status.grabState,
 707                   params->status.streamState, params->status.fatalError,
 708                   params->status.cmdError, params->status.debugFlags,
 709                   params->status.vpStatus, params->status.errorCode);
 710 }
 711
 712 static int goto_low_power(struct gspca_dev *gspca_dev)
 713 {
 714         struct sd *sd = (struct sd *) gspca_dev;
 715         int ret;
 716
 717         ret = do_command(gspca_dev, CPIA_COMMAND_GotoLoPower, 0, 0, 0, 0);
 718         if (ret)
 719                 return ret;
 720
 721         ret = do_command(gspca_dev, CPIA_COMMAND_GetCameraStatus, 0, 0, 0, 0);
 722         if (ret)
 723                 return ret;
 724
 725         if (sd->params.status.systemState != LO_POWER_STATE) {
 726                 if (sd->params.status.systemState != WARM_BOOT_STATE) {
 727                         gspca_err(gspca_dev, "unexpected state after lo power cmd: %02x\n",
 728                                   sd->params.status.systemState);
 729                         printstatus(gspca_dev, &sd->params);
 730                 }
 731                 return -EIO;
 732         }
 733
 734         gspca_dbg(gspca_dev, D_CONF, "camera now in LOW power state\n");
 735         return 0;
 736 }
 737
 738 static int goto_high_power(struct gspca_dev *gspca_dev)
 739 {
 740         struct sd *sd = (struct sd *) gspca_dev;
 741         int ret;
 742
 743         ret = do_command(gspca_dev, CPIA_COMMAND_GotoHiPower, 0, 0, 0, 0);
 744         if (ret)
 745                 return ret;
 746
 747         msleep_interruptible(40);       /* windows driver does it too */
 748
 749         if (signal_pending(current))
 750                 return -EINTR;
 751
 752         ret = do_command(gspca_dev, CPIA_COMMAND_GetCameraStatus, 0, 0, 0, 0);
 753         if (ret)
 754                 return ret;
 755
 756         if (sd->params.status.systemState != HI_POWER_STATE) {
 757                 gspca_err(gspca_dev, "unexpected state after hi power cmd: %02x\n",
 758                           sd->params.status.systemState);
 759                 printstatus(gspca_dev, &sd->params);
 760                 return -EIO;
 761         }
 762
 763         gspca_dbg(gspca_dev, D_CONF, "camera now in HIGH power state\n");
 764         return 0;
 765 }
 766
 767 static int get_version_information(struct gspca_dev *gspca_dev)
 768 {
 769         int ret;
 770
 771         /* GetCPIAVersion */
 772         ret = do_command(gspca_dev, CPIA_COMMAND_GetCPIAVersion, 0, 0, 0, 0);
 773         if (ret)
 774                 return ret;
 775
 776         /* GetPnPID */
 777         return do_command(gspca_dev, CPIA_COMMAND_GetPnPID, 0, 0, 0, 0);
 778 }
 779
 780 static int save_camera_state(struct gspca_dev *gspca_dev)
 781 {
 782         int ret;
 783
 784         ret = do_command(gspca_dev, CPIA_COMMAND_GetColourBalance, 0, 0, 0, 0);
 785         if (ret)
 786                 return ret;
 787
 788         return do_command(gspca_dev, CPIA_COMMAND_GetExposure, 0, 0, 0, 0);
 789 }
 790
 791 static int command_setformat(struct gspca_dev *gspca_dev)
 792 {
 793         struct sd *sd = (struct sd *) gspca_dev;
 794         int ret;
 795
 796         ret = do_command(gspca_dev, CPIA_COMMAND_SetFormat,
 797                          sd->params.format.videoSize,
 798                          sd->params.format.subSample,
 799                          sd->params.format.yuvOrder, 0);
 800         if (ret)
 801                 return ret;
 802
 803         return do_command(gspca_dev, CPIA_COMMAND_SetROI,
 804                           sd->params.roi.colStart, sd->params.roi.colEnd,
 805                           sd->params.roi.rowStart, sd->params.roi.rowEnd);
 806 }
 807
 808 static int command_setcolourparams(struct gspca_dev *gspca_dev)
 809 {
 810         struct sd *sd = (struct sd *) gspca_dev;
 811         return do_command(gspca_dev, CPIA_COMMAND_SetColourParams,
 812                           sd->params.colourParams.brightness,
 813                           sd->params.colourParams.contrast,
 814                           sd->params.colourParams.saturation, 0);
 815 }
 816
 817 static int command_setapcor(struct gspca_dev *gspca_dev)
 818 {
 819         struct sd *sd = (struct sd *) gspca_dev;
 820         return do_command(gspca_dev, CPIA_COMMAND_SetApcor,
 821                           sd->params.apcor.gain1,
 822                           sd->params.apcor.gain2,
 823                           sd->params.apcor.gain4,
 824                           sd->params.apcor.gain8);
 825 }
 826
 827 static int command_setvloffset(struct gspca_dev *gspca_dev)
 828 {
 829         struct sd *sd = (struct sd *) gspca_dev;
 830         return do_command(gspca_dev, CPIA_COMMAND_SetVLOffset,
 831                           sd->params.vlOffset.gain1,
 832                           sd->params.vlOffset.gain2,
 833                           sd->params.vlOffset.gain4,
 834                           sd->params.vlOffset.gain8);
 835 }
 836
 837 static int command_setexposure(struct gspca_dev *gspca_dev)
 838 {
 839         struct sd *sd = (struct sd *) gspca_dev;
 840         int ret;
 841
 842         ret = do_command_extended(gspca_dev, CPIA_COMMAND_SetExposure,
 843                                   sd->params.exposure.gainMode,
 844                                   1,
 845                                   sd->params.exposure.compMode,
 846                                   sd->params.exposure.centreWeight,
 847                                   sd->params.exposure.gain,
 848                                   sd->params.exposure.fineExp,
 849                                   sd->params.exposure.coarseExpLo,
 850                                   sd->params.exposure.coarseExpHi,
 851                                   sd->params.exposure.redComp,
 852                                   sd->params.exposure.green1Comp,
 853                                   sd->params.exposure.green2Comp,
 854                                   sd->params.exposure.blueComp);
 855         if (ret)
 856                 return ret;
 857
 858         if (sd->params.exposure.expMode != 1) {
 859                 ret = do_command_extended(gspca_dev, CPIA_COMMAND_SetExposure,
 860                                           0,
 861                                           sd->params.exposure.expMode,
 862                                           0, 0,
 863                                           sd->params.exposure.gain,
 864                                           sd->params.exposure.fineExp,
 865                                           sd->params.exposure.coarseExpLo,
 866                                           sd->params.exposure.coarseExpHi,
 867                                           0, 0, 0, 0);
 868         }
 869
 870         return ret;
 871 }
 872
 873 static int command_setcolourbalance(struct gspca_dev *gspca_dev)
 874 {
 875         struct sd *sd = (struct sd *) gspca_dev;
 876
 877         if (sd->params.colourBalance.balanceMode == 1) {
 878                 int ret;
 879
 880                 ret = do_command(gspca_dev, CPIA_COMMAND_SetColourBalance,
 881                                  1,
 882                                  sd->params.colourBalance.redGain,
 883                                  sd->params.colourBalance.greenGain,
 884                                  sd->params.colourBalance.blueGain);
 885                 if (ret)
 886                         return ret;
 887
 888                 return do_command(gspca_dev, CPIA_COMMAND_SetColourBalance,
 889                                   3, 0, 0, 0);
 890         }
 891         if (sd->params.colourBalance.balanceMode == 2) {
 892                 return do_command(gspca_dev, CPIA_COMMAND_SetColourBalance,
 893                                   2, 0, 0, 0);
 894         }
 895         if (sd->params.colourBalance.balanceMode == 3) {
 896                 return do_command(gspca_dev, CPIA_COMMAND_SetColourBalance,
 897                                   3, 0, 0, 0);
 898         }
 899
 900         return -EINVAL;
 901 }
 902
 903 static int command_setcompressiontarget(struct gspca_dev *gspca_dev)
 904 {
 905         struct sd *sd = (struct sd *) gspca_dev;
 906
 907         return do_command(gspca_dev, CPIA_COMMAND_SetCompressionTarget,
 908                           sd->params.compressionTarget.frTargeting,
 909                           sd->params.compressionTarget.targetFR,
 910                           sd->params.compressionTarget.targetQ, 0);
 911 }
 912
 913 static int command_setyuvtresh(struct gspca_dev *gspca_dev)
 914 {
 915         struct sd *sd = (struct sd *) gspca_dev;
 916
 917         return do_command(gspca_dev, CPIA_COMMAND_SetYUVThresh,
 918                           sd->params.yuvThreshold.yThreshold,
 919                           sd->params.yuvThreshold.uvThreshold, 0, 0);
 920 }
 921
 922 static int command_setcompressionparams(struct gspca_dev *gspca_dev)
 923 {
 924         struct sd *sd = (struct sd *) gspca_dev;
 925
 926         return do_command_extended(gspca_dev,
 927                             CPIA_COMMAND_SetCompressionParams,
 928                             0, 0, 0, 0,
 929                             sd->params.compressionParams.hysteresis,
 930                             sd->params.compressionParams.threshMax,
 931                             sd->params.compressionParams.smallStep,
 932                             sd->params.compressionParams.largeStep,
 933                             sd->params.compressionParams.decimationHysteresis,
 934                             sd->params.compressionParams.frDiffStepThresh,
 935                             sd->params.compressionParams.qDiffStepThresh,
 936                             sd->params.compressionParams.decimationThreshMod);
 937 }
 938
 939 static int command_setcompression(struct gspca_dev *gspca_dev)
 940 {
 941         struct sd *sd = (struct sd *) gspca_dev;
 942
 943         return do_command(gspca_dev, CPIA_COMMAND_SetCompression,
 944                           sd->params.compression.mode,
 945                           sd->params.compression.decimation, 0, 0);
 946 }
 947
 948 static int command_setsensorfps(struct gspca_dev *gspca_dev)
 949 {
 950         struct sd *sd = (struct sd *) gspca_dev;
 951
 952         return do_command(gspca_dev, CPIA_COMMAND_SetSensorFPS,
 953                           sd->params.sensorFps.divisor,
 954                           sd->params.sensorFps.baserate, 0, 0);
 955 }
 956
 957 static int command_setflickerctrl(struct gspca_dev *gspca_dev)
 958 {
 959         struct sd *sd = (struct sd *) gspca_dev;
 960
 961         return do_command(gspca_dev, CPIA_COMMAND_SetFlickerCtrl,
 962                           sd->params.flickerControl.flickerMode,
 963                           sd->params.flickerControl.coarseJump,
 964                           sd->params.flickerControl.allowableOverExposure,
 965                           0);
 966 }
 967
 968 static int command_setecptiming(struct gspca_dev *gspca_dev)
 969 {
 970         struct sd *sd = (struct sd *) gspca_dev;
 971
 972         return do_command(gspca_dev, CPIA_COMMAND_SetECPTiming,
 973                           sd->params.ecpTiming, 0, 0, 0);
 974 }
 975
 976 static int command_pause(struct gspca_dev *gspca_dev)
 977 {
 978         return do_command(gspca_dev, CPIA_COMMAND_EndStreamCap, 0, 0, 0, 0);
 979 }
 980
 981 static int command_resume(struct gspca_dev *gspca_dev)
 982 {
 983         struct sd *sd = (struct sd *) gspca_dev;
 984
 985         return do_command(gspca_dev, CPIA_COMMAND_InitStreamCap,
 986                           0, sd->params.streamStartLine, 0, 0);
 987 }
 988
 989 static int command_setlights(struct gspca_dev *gspca_dev)
 990 {
 991         struct sd *sd = (struct sd *) gspca_dev;
 992         int ret, p1, p2;
 993
 994         p1 = (sd->params.qx3.bottomlight == 0) << 1;
 995         p2 = (sd->params.qx3.toplight == 0) << 3;
 996
 997         ret = do_command(gspca_dev, CPIA_COMMAND_WriteVCReg,
 998                          0x90, 0x8f, 0x50, 0);
 999         if (ret)
1000                 return ret;
1001
1002         return do_command(gspca_dev, CPIA_COMMAND_WriteMCPort, 2, 0,
1003                           p1 | p2 | 0xe0, 0);
1004 }
1005
1006 static int set_flicker(struct gspca_dev *gspca_dev, int on, int apply)
1007 {
1008         /* Everything in here is from the Windows driver */
1009 /* define for compgain calculation */
1010 #if 0
1011 #define COMPGAIN(base, curexp, newexp) \
1012     (u8) ((((float) base - 128.0) * ((float) curexp / (float) newexp)) + 128.5)
1013 #define EXP_FROM_COMP(basecomp, curcomp, curexp) \
1014     (u16)((float)curexp * (float)(u8)(curcomp + 128) / \
1015     (float)(u8)(basecomp - 128))
1016 #else
1017   /* equivalent functions without floating point math */
1018 #define COMPGAIN(base, curexp, newexp) \
1019     (u8)(128 + (((u32)(2*(base-128)*curexp + newexp)) / (2 * newexp)))
1020 #define EXP_FROM_COMP(basecomp, curcomp, curexp) \
1021     (u16)(((u32)(curexp * (u8)(curcomp + 128)) / (u8)(basecomp - 128)))
1022 #endif
1023
1024         struct sd *sd = (struct sd *) gspca_dev;
1025         int currentexp = sd->params.exposure.coarseExpLo +
1026                          sd->params.exposure.coarseExpHi * 256;
1027         int ret, startexp;
1028
1029         if (on) {
1030                 int cj = sd->params.flickerControl.coarseJump;
1031                 sd->params.flickerControl.flickerMode = 1;
1032                 sd->params.flickerControl.disabled = 0;
1033                 if (sd->params.exposure.expMode != 2) {
1034                         sd->params.exposure.expMode = 2;
1035                         sd->exposure_status = EXPOSURE_NORMAL;
1036                 }
1037                 if (sd->params.exposure.gain >= BITS_PER_TYPE(currentexp))
1038                         return -EINVAL;
1039                 currentexp = currentexp << sd->params.exposure.gain;
1040                 sd->params.exposure.gain = 0;
1041                 /* round down current exposure to nearest value */
1042                 startexp = (currentexp + ROUND_UP_EXP_FOR_FLICKER) / cj;
1043                 if (startexp < 1)
1044                         startexp = 1;
1045                 startexp = (startexp * cj) - 1;
1046                 if (FIRMWARE_VERSION(1, 2))
1047                         while (startexp > MAX_EXP_102)
1048                                 startexp -= cj;
1049                 else
1050                         while (startexp > MAX_EXP)
1051                                 startexp -= cj;
1052                 sd->params.exposure.coarseExpLo = startexp & 0xff;
1053                 sd->params.exposure.coarseExpHi = startexp >> 8;
1054                 if (currentexp > startexp) {
1055                         if (currentexp > (2 * startexp))
1056                                 currentexp = 2 * startexp;
1057                         sd->params.exposure.redComp =
1058                                 COMPGAIN(COMP_RED, currentexp, startexp);
1059                         sd->params.exposure.green1Comp =
1060                                 COMPGAIN(COMP_GREEN1, currentexp, startexp);
1061                         sd->params.exposure.green2Comp =
1062                                 COMPGAIN(COMP_GREEN2, currentexp, startexp);
1063                         sd->params.exposure.blueComp =
1064                                 COMPGAIN(COMP_BLUE, currentexp, startexp);
1065                 } else {
1066                         sd->params.exposure.redComp = COMP_RED;
1067                         sd->params.exposure.green1Comp = COMP_GREEN1;
1068                         sd->params.exposure.green2Comp = COMP_GREEN2;
1069                         sd->params.exposure.blueComp = COMP_BLUE;
1070                 }
1071                 if (FIRMWARE_VERSION(1, 2))
1072                         sd->params.exposure.compMode = 0;
1073                 else
1074                         sd->params.exposure.compMode = 1;
1075
1076                 sd->params.apcor.gain1 = 0x18;
1077                 sd->params.apcor.gain2 = 0x18;
1078                 sd->params.apcor.gain4 = 0x16;
1079                 sd->params.apcor.gain8 = 0x14;
1080         } else {
1081                 sd->params.flickerControl.flickerMode = 0;
1082                 sd->params.flickerControl.disabled = 1;
1083                 /* Average equivalent coarse for each comp channel */
1084                 startexp = EXP_FROM_COMP(COMP_RED,
1085                                 sd->params.exposure.redComp, currentexp);
1086                 startexp += EXP_FROM_COMP(COMP_GREEN1,
1087                                 sd->params.exposure.green1Comp, currentexp);
1088                 startexp += EXP_FROM_COMP(COMP_GREEN2,
1089                                 sd->params.exposure.green2Comp, currentexp);
1090                 startexp += EXP_FROM_COMP(COMP_BLUE,
1091                                 sd->params.exposure.blueComp, currentexp);
1092                 startexp = startexp >> 2;
1093                 while (startexp > MAX_EXP && sd->params.exposure.gain <
1094                        sd->params.exposure.gainMode - 1) {
1095                         startexp = startexp >> 1;
1096                         ++sd->params.exposure.gain;
1097                 }
1098                 if (FIRMWARE_VERSION(1, 2) && startexp > MAX_EXP_102)
1099                         startexp = MAX_EXP_102;
1100                 if (startexp > MAX_EXP)
1101                         startexp = MAX_EXP;
1102                 sd->params.exposure.coarseExpLo = startexp & 0xff;
1103                 sd->params.exposure.coarseExpHi = startexp >> 8;
1104                 sd->params.exposure.redComp = COMP_RED;
1105                 sd->params.exposure.green1Comp = COMP_GREEN1;
1106                 sd->params.exposure.green2Comp = COMP_GREEN2;
1107                 sd->params.exposure.blueComp = COMP_BLUE;
1108                 sd->params.exposure.compMode = 1;
1109                 sd->params.apcor.gain1 = 0x18;
1110                 sd->params.apcor.gain2 = 0x16;
1111                 sd->params.apcor.gain4 = 0x24;
1112                 sd->params.apcor.gain8 = 0x34;
1113         }
1114         sd->params.vlOffset.gain1 = 20;
1115         sd->params.vlOffset.gain2 = 24;
1116         sd->params.vlOffset.gain4 = 26;
1117         sd->params.vlOffset.gain8 = 26;
1118
1119         if (apply) {
1120                 ret = command_setexposure(gspca_dev);
1121                 if (ret)
1122                         return ret;
1123
1124                 ret = command_setapcor(gspca_dev);
1125                 if (ret)
1126                         return ret;
1127
1128                 ret = command_setvloffset(gspca_dev);
1129                 if (ret)
1130                         return ret;
1131
1132                 ret = command_setflickerctrl(gspca_dev);
1133                 if (ret)
1134                         return ret;
1135         }
1136
1137         return 0;
1138 #undef EXP_FROM_COMP
1139 #undef COMPGAIN
1140 }
1141
1142 /* monitor the exposure and adjust the sensor frame rate if needed */
1143 static void monitor_exposure(struct gspca_dev *gspca_dev)
1144 {
1145         struct sd *sd = (struct sd *) gspca_dev;
1146         u8 exp_acc, bcomp, cmd[8];
1147         int ret, light_exp, dark_exp, very_dark_exp;
1148         int old_exposure, new_exposure, framerate;
1149         int setfps = 0, setexp = 0, setflicker = 0;
1150
1151         /* get necessary stats and register settings from camera */
1152         /* do_command can't handle this, so do it ourselves */
1153         cmd[0] = CPIA_COMMAND_ReadVPRegs >> 8;
1154         cmd[1] = CPIA_COMMAND_ReadVPRegs & 0xff;
1155         cmd[2] = 30;
1156         cmd[3] = 4;
1157         cmd[4] = 9;
1158         cmd[5] = 8;
1159         cmd[6] = 8;
1160         cmd[7] = 0;
1161         ret = cpia_usb_transferCmd(gspca_dev, cmd);
1162         if (ret) {
1163                 pr_err("ReadVPRegs(30,4,9,8) - failed: %d\n", ret);
1164                 return;
1165         }
1166         exp_acc = gspca_dev->usb_buf[0];
1167         bcomp = gspca_dev->usb_buf[1];
1168
1169         light_exp = sd->params.colourParams.brightness +
1170                     TC - 50 + EXP_ACC_LIGHT;
1171         if (light_exp > 255)
1172                 light_exp = 255;
1173         dark_exp = sd->params.colourParams.brightness +
1174                    TC - 50 - EXP_ACC_DARK;
1175         if (dark_exp < 0)
1176                 dark_exp = 0;
1177         very_dark_exp = dark_exp / 2;
1178
1179         old_exposure = sd->params.exposure.coarseExpHi * 256 +
1180                        sd->params.exposure.coarseExpLo;
1181
1182         if (!sd->params.flickerControl.disabled) {
1183                 /* Flicker control on */
1184                 int max_comp = FIRMWARE_VERSION(1, 2) ? MAX_COMP :
1185                                                         HIGH_COMP_102;
1186                 bcomp += 128;   /* decode */
1187                 if (bcomp >= max_comp && exp_acc < dark_exp) {
1188                         /* dark */
1189                         if (exp_acc < very_dark_exp) {
1190                                 /* very dark */
1191                                 if (sd->exposure_status == EXPOSURE_VERY_DARK)
1192                                         ++sd->exposure_count;
1193                                 else {
1194                                         sd->exposure_status =
1195                                                 EXPOSURE_VERY_DARK;
1196                                         sd->exposure_count = 1;
1197                                 }
1198                         } else {
1199                                 /* just dark */
1200                                 if (sd->exposure_status == EXPOSURE_DARK)
1201                                         ++sd->exposure_count;
1202                                 else {
1203                                         sd->exposure_status = EXPOSURE_DARK;
1204                                         sd->exposure_count = 1;
1205                                 }
1206                         }
1207                 } else if (old_exposure <= LOW_EXP || exp_acc > light_exp) {
1208                         /* light */
1209                         if (old_exposure <= VERY_LOW_EXP) {
1210                                 /* very light */
1211                                 if (sd->exposure_status == EXPOSURE_VERY_LIGHT)
1212                                         ++sd->exposure_count;
1213                                 else {
1214                                         sd->exposure_status =
1215                                                 EXPOSURE_VERY_LIGHT;
1216                                         sd->exposure_count = 1;
1217                                 }
1218                         } else {
1219                                 /* just light */
1220                                 if (sd->exposure_status == EXPOSURE_LIGHT)
1221                                         ++sd->exposure_count;
1222                                 else {
1223                                         sd->exposure_status = EXPOSURE_LIGHT;
1224                                         sd->exposure_count = 1;
1225                                 }
1226                         }
1227                 } else {
1228                         /* not dark or light */
1229                         sd->exposure_status = EXPOSURE_NORMAL;
1230                 }
1231         } else {
1232                 /* Flicker control off */
1233                 if (old_exposure >= MAX_EXP && exp_acc < dark_exp) {
1234                         /* dark */
1235                         if (exp_acc < very_dark_exp) {
1236                                 /* very dark */
1237                                 if (sd->exposure_status == EXPOSURE_VERY_DARK)
1238                                         ++sd->exposure_count;
1239                                 else {
1240                                         sd->exposure_status =
1241                                                 EXPOSURE_VERY_DARK;
1242                                         sd->exposure_count = 1;
1243                                 }
1244                         } else {
1245                                 /* just dark */
1246                                 if (sd->exposure_status == EXPOSURE_DARK)
1247                                         ++sd->exposure_count;
1248                                 else {
1249                                         sd->exposure_status = EXPOSURE_DARK;
1250                                         sd->exposure_count = 1;
1251                                 }
1252                         }
1253                 } else if (old_exposure <= LOW_EXP || exp_acc > light_exp) {
1254                         /* light */
1255                         if (old_exposure <= VERY_LOW_EXP) {
1256                                 /* very light */
1257                                 if (sd->exposure_status == EXPOSURE_VERY_LIGHT)
1258                                         ++sd->exposure_count;
1259                                 else {
1260                                         sd->exposure_status =
1261                                                 EXPOSURE_VERY_LIGHT;
1262                                         sd->exposure_count = 1;
1263                                 }
1264                         } else {
1265                                 /* just light */
1266                                 if (sd->exposure_status == EXPOSURE_LIGHT)
1267                                         ++sd->exposure_count;
1268                                 else {
1269                                         sd->exposure_status = EXPOSURE_LIGHT;
1270                                         sd->exposure_count = 1;
1271                                 }
1272                         }
1273                 } else {
1274                         /* not dark or light */
1275                         sd->exposure_status = EXPOSURE_NORMAL;
1276                 }
1277         }
1278
1279         framerate = atomic_read(&sd->fps);
1280         if (framerate > 30 || framerate < 1)
1281                 framerate = 1;
1282
1283         if (!sd->params.flickerControl.disabled) {
1284                 /* Flicker control on */
1285                 if ((sd->exposure_status == EXPOSURE_VERY_DARK ||
1286                      sd->exposure_status == EXPOSURE_DARK) &&
1287                     sd->exposure_count >= DARK_TIME * framerate &&
1288                     sd->params.sensorFps.divisor < 2) {
1289
1290                         /* dark for too long */
1291                         ++sd->params.sensorFps.divisor;
1292                         setfps = 1;
1293
1294                         sd->params.flickerControl.coarseJump =
1295                                 flicker_jumps[sd->mainsFreq]
1296                                              [sd->params.sensorFps.baserate]
1297                                              [sd->params.sensorFps.divisor];
1298                         setflicker = 1;
1299
1300                         new_exposure = sd->params.flickerControl.coarseJump-1;
1301                         while (new_exposure < old_exposure / 2)
1302                                 new_exposure +=
1303                                         sd->params.flickerControl.coarseJump;
1304                         sd->params.exposure.coarseExpLo = new_exposure & 0xff;
1305                         sd->params.exposure.coarseExpHi = new_exposure >> 8;
1306                         setexp = 1;
1307                         sd->exposure_status = EXPOSURE_NORMAL;
1308                         gspca_dbg(gspca_dev, D_CONF, "Automatically decreasing sensor_fps\n");
1309
1310                 } else if ((sd->exposure_status == EXPOSURE_VERY_LIGHT ||
1311                             sd->exposure_status == EXPOSURE_LIGHT) &&
1312                            sd->exposure_count >= LIGHT_TIME * framerate &&
1313                            sd->params.sensorFps.divisor > 0) {
1314
1315                         /* light for too long */
1316                         int max_exp = FIRMWARE_VERSION(1, 2) ? MAX_EXP_102 :
1317                                                                MAX_EXP;
1318                         --sd->params.sensorFps.divisor;
1319                         setfps = 1;
1320
1321                         sd->params.flickerControl.coarseJump =
1322                                 flicker_jumps[sd->mainsFreq]
1323                                              [sd->params.sensorFps.baserate]
1324                                              [sd->params.sensorFps.divisor];
1325                         setflicker = 1;
1326
1327                         new_exposure = sd->params.flickerControl.coarseJump-1;
1328                         while (new_exposure < 2 * old_exposure &&
1329                                new_exposure +
1330                                sd->params.flickerControl.coarseJump < max_exp)
1331                                 new_exposure +=
1332                                         sd->params.flickerControl.coarseJump;
1333                         sd->params.exposure.coarseExpLo = new_exposure & 0xff;
1334                         sd->params.exposure.coarseExpHi = new_exposure >> 8;
1335                         setexp = 1;
1336                         sd->exposure_status = EXPOSURE_NORMAL;
1337                         gspca_dbg(gspca_dev, D_CONF, "Automatically increasing sensor_fps\n");
1338                 }
1339         } else {
1340                 /* Flicker control off */
1341                 if ((sd->exposure_status == EXPOSURE_VERY_DARK ||
1342                      sd->exposure_status == EXPOSURE_DARK) &&
1343                     sd->exposure_count >= DARK_TIME * framerate &&
1344                     sd->params.sensorFps.divisor < 2) {
1345
1346                         /* dark for too long */
1347                         ++sd->params.sensorFps.divisor;
1348                         setfps = 1;
1349
1350                         if (sd->params.exposure.gain > 0) {
1351                                 --sd->params.exposure.gain;
1352                                 setexp = 1;
1353                         }
1354                         sd->exposure_status = EXPOSURE_NORMAL;
1355                         gspca_dbg(gspca_dev, D_CONF, "Automatically decreasing sensor_fps\n");
1356
1357                 } else if ((sd->exposure_status == EXPOSURE_VERY_LIGHT ||
1358                             sd->exposure_status == EXPOSURE_LIGHT) &&
1359                            sd->exposure_count >= LIGHT_TIME * framerate &&
1360                            sd->params.sensorFps.divisor > 0) {
1361
1362                         /* light for too long */
1363                         --sd->params.sensorFps.divisor;
1364                         setfps = 1;
1365
1366                         if (sd->params.exposure.gain <
1367                             sd->params.exposure.gainMode - 1) {
1368                                 ++sd->params.exposure.gain;
1369                                 setexp = 1;
1370                         }
1371                         sd->exposure_status = EXPOSURE_NORMAL;
1372                         gspca_dbg(gspca_dev, D_CONF, "Automatically increasing sensor_fps\n");
1373                 }
1374         }
1375
1376         if (setexp)
1377                 command_setexposure(gspca_dev);
1378
1379         if (setfps)
1380                 command_setsensorfps(gspca_dev);
1381
1382         if (setflicker)
1383                 command_setflickerctrl(gspca_dev);
1384 }
1385
1386 /*-----------------------------------------------------------------*/
1387 /* if flicker is switched off, this function switches it back on.It checks,
1388    however, that conditions are suitable before restarting it.
1389    This should only be called for firmware version 1.2.
1390
1391    It also adjust the colour balance when an exposure step is detected - as
1392    long as flicker is running
1393 */
1394 static void restart_flicker(struct gspca_dev *gspca_dev)
1395 {
1396         struct sd *sd = (struct sd *) gspca_dev;
1397         int cam_exposure, old_exp;
1398
1399         if (!FIRMWARE_VERSION(1, 2))
1400                 return;
1401
1402         cam_exposure = atomic_read(&sd->cam_exposure);
1403
1404         if (sd->params.flickerControl.flickerMode == 0 ||
1405             cam_exposure == 0)
1406                 return;
1407
1408         old_exp = sd->params.exposure.coarseExpLo +
1409                   sd->params.exposure.coarseExpHi*256;
1410         /*
1411           see how far away camera exposure is from a valid
1412           flicker exposure value
1413         */
1414         cam_exposure %= sd->params.flickerControl.coarseJump;
1415         if (!sd->params.flickerControl.disabled &&
1416             cam_exposure <= sd->params.flickerControl.coarseJump - 3) {
1417                 /* Flicker control auto-disabled */
1418                 sd->params.flickerControl.disabled = 1;
1419         }
1420
1421         if (sd->params.flickerControl.disabled &&
1422             old_exp > sd->params.flickerControl.coarseJump +
1423                       ROUND_UP_EXP_FOR_FLICKER) {
1424                 /* exposure is now high enough to switch
1425                    flicker control back on */
1426                 set_flicker(gspca_dev, 1, 1);
1427         }
1428 }
1429
1430 /* this function is called at probe time */
1431 static int sd_config(struct gspca_dev *gspca_dev,
1432                         const struct usb_device_id *id)
1433 {
1434         struct sd *sd = (struct sd *) gspca_dev;
1435         struct cam *cam;
1436
1437         sd->mainsFreq = FREQ_DEF == V4L2_CID_POWER_LINE_FREQUENCY_60HZ;
1438         reset_camera_params(gspca_dev);
1439
1440         gspca_dbg(gspca_dev, D_PROBE, "cpia CPiA camera detected (vid/pid 0x%04X:0x%04X)\n",
1441                   id->idVendor, id->idProduct);
1442
1443         cam = &gspca_dev->cam;
1444         cam->cam_mode = mode;
1445         cam->nmodes = ARRAY_SIZE(mode);
1446
1447         goto_low_power(gspca_dev);
1448         /* Check the firmware version. */
1449         sd->params.version.firmwareVersion = 0;
1450         get_version_information(gspca_dev);
1451         if (sd->params.version.firmwareVersion != 1) {
1452                 gspca_err(gspca_dev, "only firmware version 1 is supported (got: %d)\n",
1453                           sd->params.version.firmwareVersion);
1454                 return -ENODEV;
1455         }
1456
1457         /* A bug in firmware 1-02 limits gainMode to 2 */
1458         if (sd->params.version.firmwareRevision <= 2 &&
1459             sd->params.exposure.gainMode > 2) {
1460                 sd->params.exposure.gainMode = 2;
1461         }
1462
1463         /* set QX3 detected flag */
1464         sd->params.qx3.qx3_detected = (sd->params.pnpID.vendor == 0x0813 &&
1465                                        sd->params.pnpID.product == 0x0001);
1466         return 0;
1467 }
1468
1469 /* -- start the camera -- */
1470 static int sd_start(struct gspca_dev *gspca_dev)
1471 {
1472         struct sd *sd = (struct sd *) gspca_dev;
1473         int priv, ret;
1474
1475         /* Start the camera in low power mode */
1476         if (goto_low_power(gspca_dev)) {
1477                 if (sd->params.status.systemState != WARM_BOOT_STATE) {
1478                         gspca_err(gspca_dev, "unexpected systemstate: %02x\n",
1479                                   sd->params.status.systemState);
1480                         printstatus(gspca_dev, &sd->params);
1481                         return -ENODEV;
1482                 }
1483
1484                 /* FIXME: this is just dirty trial and error */
1485                 ret = goto_high_power(gspca_dev);
1486                 if (ret)
1487                         return ret;
1488
1489                 ret = do_command(gspca_dev, CPIA_COMMAND_DiscardFrame,
1490                                  0, 0, 0, 0);
1491                 if (ret)
1492                         return ret;
1493
1494                 ret = goto_low_power(gspca_dev);
1495                 if (ret)
1496                         return ret;
1497         }
1498
1499         /* procedure described in developer's guide p3-28 */
1500
1501         /* Check the firmware version. */
1502         sd->params.version.firmwareVersion = 0;
1503         get_version_information(gspca_dev);
1504
1505         /* The fatal error checking should be done after
1506          * the camera powers up (developer's guide p 3-38) */
1507
1508         /* Set streamState before transition to high power to avoid bug
1509          * in firmware 1-02 */
1510         ret = do_command(gspca_dev, CPIA_COMMAND_ModifyCameraStatus,
1511                          STREAMSTATE, 0, STREAM_NOT_READY, 0);
1512         if (ret)
1513                 return ret;
1514
1515         /* GotoHiPower */
1516         ret = goto_high_power(gspca_dev);
1517         if (ret)
1518                 return ret;
1519
1520         /* Check the camera status */
1521         ret = do_command(gspca_dev, CPIA_COMMAND_GetCameraStatus, 0, 0, 0, 0);
1522         if (ret)
1523                 return ret;
1524
1525         if (sd->params.status.fatalError) {
1526                 gspca_err(gspca_dev, "fatal_error: %04x, vp_status: %04x\n",
1527                           sd->params.status.fatalError,
1528                           sd->params.status.vpStatus);
1529                 return -EIO;
1530         }
1531
1532         /* VPVersion can't be retrieved before the camera is in HiPower,
1533          * so get it here instead of in get_version_information. */
1534         ret = do_command(gspca_dev, CPIA_COMMAND_GetVPVersion, 0, 0, 0, 0);
1535         if (ret)
1536                 return ret;
1537
1538         /* Determine video mode settings */
1539         sd->params.streamStartLine = 120;
1540
1541         priv = gspca_dev->cam.cam_mode[gspca_dev->curr_mode].priv;
1542         if (priv & 0x01) { /* crop */
1543                 sd->params.roi.colStart = 2;
1544                 sd->params.roi.rowStart = 6;
1545         } else {
1546                 sd->params.roi.colStart = 0;
1547                 sd->params.roi.rowStart = 0;
1548         }
1549
1550         if (priv & 0x02) { /* quarter */
1551                 sd->params.format.videoSize = VIDEOSIZE_QCIF;
1552                 sd->params.roi.colStart /= 2;
1553                 sd->params.roi.rowStart /= 2;
1554                 sd->params.streamStartLine /= 2;
1555         } else
1556                 sd->params.format.videoSize = VIDEOSIZE_CIF;
1557
1558         sd->params.roi.colEnd = sd->params.roi.colStart +
1559                                 (gspca_dev->pixfmt.width >> 3);
1560         sd->params.roi.rowEnd = sd->params.roi.rowStart +
1561                                 (gspca_dev->pixfmt.height >> 2);
1562
1563         /* And now set the camera to a known state */
1564         ret = do_command(gspca_dev, CPIA_COMMAND_SetGrabMode,
1565                          CPIA_GRAB_CONTINEOUS, 0, 0, 0);
1566         if (ret)
1567                 return ret;
1568         /* We start with compression disabled, as we need one uncompressed
1569            frame to handle later compressed frames */
1570         ret = do_command(gspca_dev, CPIA_COMMAND_SetCompression,
1571                          CPIA_COMPRESSION_NONE,
1572                          NO_DECIMATION, 0, 0);
1573         if (ret)
1574                 return ret;
1575         ret = command_setcompressiontarget(gspca_dev);
1576         if (ret)
1577                 return ret;
1578         ret = command_setcolourparams(gspca_dev);
1579         if (ret)
1580                 return ret;
1581         ret = command_setformat(gspca_dev);
1582         if (ret)
1583                 return ret;
1584         ret = command_setyuvtresh(gspca_dev);
1585         if (ret)
1586                 return ret;
1587         ret = command_setecptiming(gspca_dev);
1588         if (ret)
1589                 return ret;
1590         ret = command_setcompressionparams(gspca_dev);
1591         if (ret)
1592                 return ret;
1593         ret = command_setexposure(gspca_dev);
1594         if (ret)
1595                 return ret;
1596         ret = command_setcolourbalance(gspca_dev);
1597         if (ret)
1598                 return ret;
1599         ret = command_setsensorfps(gspca_dev);
1600         if (ret)
1601                 return ret;
1602         ret = command_setapcor(gspca_dev);
1603         if (ret)
1604                 return ret;
1605         ret = command_setflickerctrl(gspca_dev);
1606         if (ret)
1607                 return ret;
1608         ret = command_setvloffset(gspca_dev);
1609         if (ret)
1610                 return ret;
1611
1612         /* Start stream */
1613         ret = command_resume(gspca_dev);
1614         if (ret)
1615                 return ret;
1616
1617         /* Wait 6 frames before turning compression on for the sensor to get
1618            all settings and AEC/ACB to settle */
1619         sd->first_frame = 6;
1620         sd->exposure_status = EXPOSURE_NORMAL;
1621         sd->exposure_count = 0;
1622         atomic_set(&sd->cam_exposure, 0);
1623         atomic_set(&sd->fps, 0);
1624
1625         return 0;
1626 }
1627
1628 static void sd_stopN(struct gspca_dev *gspca_dev)
1629 {
1630         struct sd *sd __maybe_unused = (struct sd *) gspca_dev;
1631
1632         command_pause(gspca_dev);
1633
1634         /* save camera state for later open (developers guide ch 3.5.3) */
1635         save_camera_state(gspca_dev);
1636
1637         /* GotoLoPower */
1638         goto_low_power(gspca_dev);
1639
1640         /* Update the camera status */
1641         do_command(gspca_dev, CPIA_COMMAND_GetCameraStatus, 0, 0, 0, 0);
1642
1643 #if IS_ENABLED(CONFIG_INPUT)
1644         /* If the last button state is pressed, release it now! */
1645         if (sd->params.qx3.button) {
1646                 /* The camera latch will hold the pressed state until we reset
1647                    the latch, so we do not reset sd->params.qx3.button now, to
1648                    avoid a false keypress being reported the next sd_start */
1649                 input_report_key(gspca_dev->input_dev, KEY_CAMERA, 0);
1650                 input_sync(gspca_dev->input_dev);
1651         }
1652 #endif
1653 }
1654
1655 /* this function is called at probe and resume time */
1656 static int sd_init(struct gspca_dev *gspca_dev)
1657 {
1658         struct sd *sd = (struct sd *) gspca_dev;
1659         int ret;
1660
1661         /* Start / Stop the camera to make sure we are talking to
1662            a supported camera, and to get some information from it
1663            to print. */
1664         ret = sd_start(gspca_dev);
1665         if (ret)
1666                 return ret;
1667
1668         /* Ensure the QX3 illuminators' states are restored upon resume,
1669            or disable the illuminator controls, if this isn't a QX3 */
1670         if (sd->params.qx3.qx3_detected)
1671                 command_setlights(gspca_dev);
1672
1673         sd_stopN(gspca_dev);
1674
1675         gspca_dbg(gspca_dev, D_PROBE, "CPIA Version:             %d.%02d (%d.%d)\n",
1676                   sd->params.version.firmwareVersion,
1677                   sd->params.version.firmwareRevision,
1678                   sd->params.version.vcVersion,
1679                   sd->params.version.vcRevision);
1680         gspca_dbg(gspca_dev, D_PROBE, "CPIA PnP-ID:              %04x:%04x:%04x",
1681                   sd->params.pnpID.vendor, sd->params.pnpID.product,
1682                   sd->params.pnpID.deviceRevision);
1683         gspca_dbg(gspca_dev, D_PROBE, "VP-Version:               %d.%d %04x",
1684                   sd->params.vpVersion.vpVersion,
1685                   sd->params.vpVersion.vpRevision,
1686                   sd->params.vpVersion.cameraHeadID);
1687
1688         return 0;
1689 }
1690
1691 static void sd_pkt_scan(struct gspca_dev *gspca_dev,
1692                         u8 *data,
1693                         int len)
1694 {
1695         struct sd *sd = (struct sd *) gspca_dev;
1696
1697         /* Check for SOF */
1698         if (len >= 64 &&
1699             data[0] == MAGIC_0 && data[1] == MAGIC_1 &&
1700             data[16] == sd->params.format.videoSize &&
1701             data[17] == sd->params.format.subSample &&
1702             data[18] == sd->params.format.yuvOrder &&
1703             data[24] == sd->params.roi.colStart &&
1704             data[25] == sd->params.roi.colEnd &&
1705             data[26] == sd->params.roi.rowStart &&
1706             data[27] == sd->params.roi.rowEnd) {
1707                 u8 *image;
1708
1709                 atomic_set(&sd->cam_exposure, data[39] * 2);
1710                 atomic_set(&sd->fps, data[41]);
1711
1712                 /* Check for proper EOF for last frame */
1713                 image = gspca_dev->image;
1714                 if (image != NULL &&
1715                     gspca_dev->image_len > 4 &&
1716                     image[gspca_dev->image_len - 4] == 0xff &&
1717                     image[gspca_dev->image_len - 3] == 0xff &&
1718                     image[gspca_dev->image_len - 2] == 0xff &&
1719                     image[gspca_dev->image_len - 1] == 0xff)
1720                         gspca_frame_add(gspca_dev, LAST_PACKET,
1721                                                 NULL, 0);
1722
1723                 gspca_frame_add(gspca_dev, FIRST_PACKET, data, len);
1724                 return;
1725         }
1726
1727         gspca_frame_add(gspca_dev, INTER_PACKET, data, len);
1728 }
1729
1730 static void sd_dq_callback(struct gspca_dev *gspca_dev)
1731 {
1732         struct sd *sd = (struct sd *) gspca_dev;
1733
1734         /* Set the normal compression settings once we have captured a
1735            few uncompressed frames (and AEC has hopefully settled) */
1736         if (sd->first_frame) {
1737                 sd->first_frame--;
1738                 if (sd->first_frame == 0)
1739                         command_setcompression(gspca_dev);
1740         }
1741
1742         /* Switch flicker control back on if it got turned off */
1743         restart_flicker(gspca_dev);
1744
1745         /* If AEC is enabled, monitor the exposure and
1746            adjust the sensor frame rate if needed */
1747         if (sd->params.exposure.expMode == 2)
1748                 monitor_exposure(gspca_dev);
1749
1750         /* Update our knowledge of the camera state */
1751         do_command(gspca_dev, CPIA_COMMAND_GetExposure, 0, 0, 0, 0);
1752         do_command(gspca_dev, CPIA_COMMAND_ReadMCPorts, 0, 0, 0, 0);
1753 }
1754
1755 static int sd_s_ctrl(struct v4l2_ctrl *ctrl)
1756 {
1757         struct gspca_dev *gspca_dev =
1758                 container_of(ctrl->handler, struct gspca_dev, ctrl_handler);
1759         struct sd *sd = (struct sd *)gspca_dev;
1760
1761         gspca_dev->usb_err = 0;
1762
1763         if (!gspca_dev->streaming && ctrl->id != V4L2_CID_POWER_LINE_FREQUENCY)
1764                 return 0;
1765
1766         switch (ctrl->id) {
1767         case V4L2_CID_BRIGHTNESS:
1768                 sd->params.colourParams.brightness = ctrl->val;
1769                 sd->params.flickerControl.allowableOverExposure =
1770                         find_over_exposure(sd->params.colourParams.brightness);
1771                 gspca_dev->usb_err = command_setcolourparams(gspca_dev);
1772                 if (!gspca_dev->usb_err)
1773                         gspca_dev->usb_err = command_setflickerctrl(gspca_dev);
1774                 break;
1775         case V4L2_CID_CONTRAST:
1776                 sd->params.colourParams.contrast = ctrl->val;
1777                 gspca_dev->usb_err = command_setcolourparams(gspca_dev);
1778                 break;
1779         case V4L2_CID_SATURATION:
1780                 sd->params.colourParams.saturation = ctrl->val;
1781                 gspca_dev->usb_err = command_setcolourparams(gspca_dev);
1782                 break;
1783         case V4L2_CID_POWER_LINE_FREQUENCY:
1784                 sd->mainsFreq = ctrl->val == V4L2_CID_POWER_LINE_FREQUENCY_60HZ;
1785                 sd->params.flickerControl.coarseJump =
1786                         flicker_jumps[sd->mainsFreq]
1787                         [sd->params.sensorFps.baserate]
1788                         [sd->params.sensorFps.divisor];
1789
1790                 gspca_dev->usb_err = set_flicker(gspca_dev,
1791                         ctrl->val != V4L2_CID_POWER_LINE_FREQUENCY_DISABLED,
1792                         gspca_dev->streaming);
1793                 break;
1794         case V4L2_CID_ILLUMINATORS_1:
1795                 sd->params.qx3.bottomlight = ctrl->val;
1796                 gspca_dev->usb_err = command_setlights(gspca_dev);
1797                 break;
1798         case V4L2_CID_ILLUMINATORS_2:
1799                 sd->params.qx3.toplight = ctrl->val;
1800                 gspca_dev->usb_err = command_setlights(gspca_dev);
1801                 break;
1802         case CPIA1_CID_COMP_TARGET:
1803                 sd->params.compressionTarget.frTargeting = ctrl->val;
1804                 gspca_dev->usb_err = command_setcompressiontarget(gspca_dev);
1805                 break;
1806         }
1807         return gspca_dev->usb_err;
1808 }
1809
1810 static const struct v4l2_ctrl_ops sd_ctrl_ops = {
1811         .s_ctrl = sd_s_ctrl,
1812 };
1813
1814 static int sd_init_controls(struct gspca_dev *gspca_dev)
1815 {
1816         struct sd *sd = (struct sd *)gspca_dev;
1817         struct v4l2_ctrl_handler *hdl = &gspca_dev->ctrl_handler;
1818         static const char * const comp_target_menu[] = {
1819                 "Quality",
1820                 "Framerate",
1821                 NULL
1822         };
1823         static const struct v4l2_ctrl_config comp_target = {
1824                 .ops = &sd_ctrl_ops,
1825                 .id = CPIA1_CID_COMP_TARGET,
1826                 .type = V4L2_CTRL_TYPE_MENU,
1827                 .name = "Compression Target",
1828                 .qmenu = comp_target_menu,
1829                 .max = 1,
1830                 .def = COMP_TARGET_DEF,
1831         };
1832
1833         gspca_dev->vdev.ctrl_handler = hdl;
1834         v4l2_ctrl_handler_init(hdl, 7);
1835         v4l2_ctrl_new_std(hdl, &sd_ctrl_ops,
1836                         V4L2_CID_BRIGHTNESS, 0, 100, 1, BRIGHTNESS_DEF);
1837         v4l2_ctrl_new_std(hdl, &sd_ctrl_ops,
1838                         V4L2_CID_CONTRAST, 0, 96, 8, CONTRAST_DEF);
1839         v4l2_ctrl_new_std(hdl, &sd_ctrl_ops,
1840                         V4L2_CID_SATURATION, 0, 100, 1, SATURATION_DEF);
1841         sd->freq = v4l2_ctrl_new_std_menu(hdl, &sd_ctrl_ops,
1842                         V4L2_CID_POWER_LINE_FREQUENCY,
1843                         V4L2_CID_POWER_LINE_FREQUENCY_60HZ, 0,
1844                         FREQ_DEF);
1845         if (sd->params.qx3.qx3_detected) {
1846                 v4l2_ctrl_new_std(hdl, &sd_ctrl_ops,
1847                                 V4L2_CID_ILLUMINATORS_1, 0, 1, 1,
1848                                 ILLUMINATORS_1_DEF);
1849                 v4l2_ctrl_new_std(hdl, &sd_ctrl_ops,
1850                                 V4L2_CID_ILLUMINATORS_2, 0, 1, 1,
1851                                 ILLUMINATORS_2_DEF);
1852         }
1853         v4l2_ctrl_new_custom(hdl, &comp_target, NULL);
1854
1855         if (hdl->error) {
1856                 pr_err("Could not initialize controls\n");
1857                 return hdl->error;
1858         }
1859         return 0;
1860 }
1861
1862 /* sub-driver description */
1863 static const struct sd_desc sd_desc = {
1864         .name = MODULE_NAME,
1865         .config = sd_config,
1866         .init = sd_init,
1867         .init_controls = sd_init_controls,
1868         .start = sd_start,
1869         .stopN = sd_stopN,
1870         .dq_callback = sd_dq_callback,
1871         .pkt_scan = sd_pkt_scan,
1872 #if IS_ENABLED(CONFIG_INPUT)
1873         .other_input = 1,
1874 #endif
1875 };
1876
1877 /* -- module initialisation -- */
1878 static const struct usb_device_id device_table[] = {
1879         {USB_DEVICE(0x0553, 0x0002)},
1880         {USB_DEVICE(0x0813, 0x0001)},
1881         {}
1882 };
1883 MODULE_DEVICE_TABLE(usb, device_table);
1884
1885 /* -- device connect -- */
1886 static int sd_probe(struct usb_interface *intf,
1887                         const struct usb_device_id *id)
1888 {
1889         return gspca_dev_probe(intf, id, &sd_desc, sizeof(struct sd),
1890                                 THIS_MODULE);
1891 }
1892
1893 static struct usb_driver sd_driver = {
1894         .name = MODULE_NAME,
1895         .id_table = device_table,
1896         .probe = sd_probe,
1897         .disconnect = gspca_disconnect,
1898 #ifdef CONFIG_PM
1899         .suspend = gspca_suspend,
1900         .resume = gspca_resume,
1901         .reset_resume = gspca_resume,
1902 #endif
1903 };
1904
1905 module_usb_driver(sd_driver);