drivers/media/usb/gspca/m5602/m5602_po1030.c

   1 /*
   2  * Driver for the po1030 sensor
   3  *
   4  * Copyright (c) 2008 Erik Andrén
   5  * Copyright (c) 2007 Ilyes Gouta. Based on the m5603x Linux Driver Project.
   6  * Copyright (c) 2005 m5603x Linux Driver Project <m5602@x3ng.com.br>
   7  *
   8  * Portions of code to USB interface and ALi driver software,
   9  * Copyright (c) 2006 Willem Duinker
  10  * v4l2 interface modeled after the V4L2 driver
  11  * for SN9C10x PC Camera Controllers
  12  *
  13  * This program is free software; you can redistribute it and/or
  14  * modify it under the terms of the GNU General Public License as
  15  * published by the Free Software Foundation, version 2.
  16  *
  17  */
  18
  19 #define pr_fmt(fmt) KBUILD_MODNAME ": " fmt
  20
  21 #include "m5602_po1030.h"
  22
  23 static int po1030_s_ctrl(struct v4l2_ctrl *ctrl);
  24 static void po1030_dump_registers(struct sd *sd);
  25
  26 static const unsigned char preinit_po1030[][3] = {
  27         {BRIDGE, M5602_XB_MCU_CLK_DIV, 0x02},
  28         {BRIDGE, M5602_XB_MCU_CLK_CTRL, 0xb0},
  29         {BRIDGE, M5602_XB_SEN_CLK_DIV, 0x00},
  30         {BRIDGE, M5602_XB_SEN_CLK_CTRL, 0xb0},
  31         {BRIDGE, M5602_XB_ADC_CTRL, 0xc0},
  32         {BRIDGE, M5602_XB_SENSOR_CTRL, 0x00},
  33         {BRIDGE, M5602_XB_SENSOR_TYPE, 0x0c},
  34         {BRIDGE, M5602_XB_ADC_CTRL, 0xc0},
  35         {BRIDGE, M5602_XB_GPIO_DIR, 0x05},
  36         {BRIDGE, M5602_XB_GPIO_DAT, 0x04},
  37         {BRIDGE, M5602_XB_GPIO_EN_H, 0x06},
  38         {BRIDGE, M5602_XB_GPIO_DIR_H, 0x06},
  39         {BRIDGE, M5602_XB_GPIO_DAT_H, 0x02},
  40
  41         {SENSOR, PO1030_AUTOCTRL2, PO1030_SENSOR_RESET | (1 << 2)},
  42
  43         {BRIDGE, M5602_XB_SEN_CLK_DIV, 0x04},
  44         {BRIDGE, M5602_XB_SEN_CLK_CTRL, 0xb0},
  45         {BRIDGE, M5602_XB_SEN_CLK_DIV, 0x00},
  46         {BRIDGE, M5602_XB_SEN_CLK_CTRL, 0xb0},
  47         {BRIDGE, M5602_XB_SENSOR_TYPE, 0x0c},
  48         {BRIDGE, M5602_XB_GPIO_DIR, 0x05},
  49         {BRIDGE, M5602_XB_GPIO_DAT, 0x00}
  50 };
  51
  52 static const unsigned char init_po1030[][3] = {
  53         {BRIDGE, M5602_XB_MCU_CLK_DIV, 0x02},
  54         {BRIDGE, M5602_XB_MCU_CLK_CTRL, 0xb0},
  55         {BRIDGE, M5602_XB_SEN_CLK_DIV, 0x00},
  56         {BRIDGE, M5602_XB_SEN_CLK_CTRL, 0xb0},
  57         {BRIDGE, M5602_XB_ADC_CTRL, 0xc0},
  58         {BRIDGE, M5602_XB_SENSOR_CTRL, 0x00},
  59         {BRIDGE, M5602_XB_SENSOR_TYPE, 0x0c},
  60
  61         {SENSOR, PO1030_AUTOCTRL2, PO1030_SENSOR_RESET | (1 << 2)},
  62
  63         {BRIDGE, M5602_XB_GPIO_DIR, 0x05},
  64         {BRIDGE, M5602_XB_GPIO_DAT, 0x04},
  65         {BRIDGE, M5602_XB_GPIO_EN_H, 0x06},
  66         {BRIDGE, M5602_XB_GPIO_EN_L, 0x00},
  67         {BRIDGE, M5602_XB_GPIO_DIR_H, 0x06},
  68         {BRIDGE, M5602_XB_GPIO_DAT_H, 0x02},
  69         {BRIDGE, M5602_XB_SEN_CLK_DIV, 0x04},
  70         {BRIDGE, M5602_XB_SEN_CLK_CTRL, 0xb0},
  71         {BRIDGE, M5602_XB_GPIO_DIR, 0x05},
  72         {BRIDGE, M5602_XB_GPIO_DAT, 0x00},
  73
  74         {SENSOR, PO1030_AUTOCTRL2, 0x04},
  75
  76         {SENSOR, PO1030_OUTFORMCTRL2, PO1030_RAW_RGB_BAYER},
  77         {SENSOR, PO1030_AUTOCTRL1, PO1030_WEIGHT_WIN_2X},
  78
  79         {SENSOR, PO1030_CONTROL2, 0x03},
  80         {SENSOR, 0x21, 0x90},
  81         {SENSOR, PO1030_YTARGET, 0x60},
  82         {SENSOR, 0x59, 0x13},
  83         {SENSOR, PO1030_OUTFORMCTRL1, PO1030_HREF_ENABLE},
  84         {SENSOR, PO1030_EDGE_ENH_OFF, 0x00},
  85         {SENSOR, PO1030_EGA, 0x80},
  86         {SENSOR, 0x78, 0x14},
  87         {SENSOR, 0x6f, 0x01},
  88         {SENSOR, PO1030_GLOBALGAINMAX, 0x14},
  89         {SENSOR, PO1030_Cb_U_GAIN, 0x38},
  90         {SENSOR, PO1030_Cr_V_GAIN, 0x38},
  91         {SENSOR, PO1030_CONTROL1, PO1030_SHUTTER_MODE |
  92                                   PO1030_AUTO_SUBSAMPLING |
  93                                   PO1030_FRAME_EQUAL},
  94         {SENSOR, PO1030_GC0, 0x10},
  95         {SENSOR, PO1030_GC1, 0x20},
  96         {SENSOR, PO1030_GC2, 0x40},
  97         {SENSOR, PO1030_GC3, 0x60},
  98         {SENSOR, PO1030_GC4, 0x80},
  99         {SENSOR, PO1030_GC5, 0xa0},
 100         {SENSOR, PO1030_GC6, 0xc0},
 101         {SENSOR, PO1030_GC7, 0xff},
 102
 103         /* Set the width to 751 */
 104         {SENSOR, PO1030_FRAMEWIDTH_H, 0x02},
 105         {SENSOR, PO1030_FRAMEWIDTH_L, 0xef},
 106
 107         /* Set the height to 540 */
 108         {SENSOR, PO1030_FRAMEHEIGHT_H, 0x02},
 109         {SENSOR, PO1030_FRAMEHEIGHT_L, 0x1c},
 110
 111         /* Set the x window to 1 */
 112         {SENSOR, PO1030_WINDOWX_H, 0x00},
 113         {SENSOR, PO1030_WINDOWX_L, 0x01},
 114
 115         /* Set the y window to 1 */
 116         {SENSOR, PO1030_WINDOWY_H, 0x00},
 117         {SENSOR, PO1030_WINDOWY_L, 0x01},
 118
 119         /* with a very low lighted environment increase the exposure but
 120          * decrease the FPS (Frame Per Second) */
 121         {BRIDGE, M5602_XB_SEN_CLK_DIV, 0x00},
 122         {BRIDGE, M5602_XB_SEN_CLK_CTRL, 0xb0},
 123
 124         {BRIDGE, M5602_XB_GPIO_DIR, 0x05},
 125         {BRIDGE, M5602_XB_GPIO_DAT, 0x00},
 126         {BRIDGE, M5602_XB_GPIO_EN_H, 0x06},
 127         {BRIDGE, M5602_XB_GPIO_EN_L, 0x00},
 128 };
 129
 130 static struct v4l2_pix_format po1030_modes[] = {
 131         {
 132                 640,
 133                 480,
 134                 V4L2_PIX_FMT_SBGGR8,
 135                 V4L2_FIELD_NONE,
 136                 .sizeimage = 640 * 480,
 137                 .bytesperline = 640,
 138                 .colorspace = V4L2_COLORSPACE_SRGB,
 139                 .priv = 2
 140         }
 141 };
 142
 143 static const struct v4l2_ctrl_ops po1030_ctrl_ops = {
 144         .s_ctrl = po1030_s_ctrl,
 145 };
 146
 147 static const struct v4l2_ctrl_config po1030_greenbal_cfg = {
 148         .ops    = &po1030_ctrl_ops,
 149         .id     = M5602_V4L2_CID_GREEN_BALANCE,
 150         .name   = "Green Balance",
 151         .type   = V4L2_CTRL_TYPE_INTEGER,
 152         .min    = 0,
 153         .max    = 255,
 154         .step   = 1,
 155         .def    = PO1030_GREEN_GAIN_DEFAULT,
 156         .flags  = V4L2_CTRL_FLAG_SLIDER,
 157 };
 158
 159 int po1030_probe(struct sd *sd)
 160 {
 161         u8 dev_id_h = 0, i;
 162         int err;
 163         struct gspca_dev *gspca_dev = (struct gspca_dev *)sd;
 164
 165         if (force_sensor) {
 166                 if (force_sensor == PO1030_SENSOR) {
 167                         pr_info("Forcing a %s sensor\n", po1030.name);
 168                         goto sensor_found;
 169                 }
 170                 /* If we want to force another sensor, don't try to probe this
 171                  * one */
 172                 return -ENODEV;
 173         }
 174
 175         gspca_dbg(gspca_dev, D_PROBE, "Probing for a po1030 sensor\n");
 176
 177         /* Run the pre-init to actually probe the unit */
 178         for (i = 0; i < ARRAY_SIZE(preinit_po1030); i++) {
 179                 u8 data = preinit_po1030[i][2];
 180                 if (preinit_po1030[i][0] == SENSOR)
 181                         err = m5602_write_sensor(sd, preinit_po1030[i][1],
 182                                                  &data, 1);
 183                 else
 184                         err = m5602_write_bridge(sd, preinit_po1030[i][1],
 185                                                  data);
 186                 if (err < 0)
 187                         return err;
 188         }
 189
 190         if (m5602_read_sensor(sd, PO1030_DEVID_H, &dev_id_h, 1))
 191                 return -ENODEV;
 192
 193         if (dev_id_h == 0x30) {
 194                 pr_info("Detected a po1030 sensor\n");
 195                 goto sensor_found;
 196         }
 197         return -ENODEV;
 198
 199 sensor_found:
 200         sd->gspca_dev.cam.cam_mode = po1030_modes;
 201         sd->gspca_dev.cam.nmodes = ARRAY_SIZE(po1030_modes);
 202
 203         return 0;
 204 }
 205
 206 int po1030_init(struct sd *sd)
 207 {
 208         int i, err = 0;
 209
 210         /* Init the sensor */
 211         for (i = 0; i < ARRAY_SIZE(init_po1030) && !err; i++) {
 212                 u8 data[2] = {0x00, 0x00};
 213
 214                 switch (init_po1030[i][0]) {
 215                 case BRIDGE:
 216                         err = m5602_write_bridge(sd,
 217                                 init_po1030[i][1],
 218                                 init_po1030[i][2]);
 219                         break;
 220
 221                 case SENSOR:
 222                         data[0] = init_po1030[i][2];
 223                         err = m5602_write_sensor(sd,
 224                                 init_po1030[i][1], data, 1);
 225                         break;
 226
 227                 default:
 228                         pr_info("Invalid stream command, exiting init\n");
 229                         return -EINVAL;
 230                 }
 231         }
 232         if (err < 0)
 233                 return err;
 234
 235         if (dump_sensor)
 236                 po1030_dump_registers(sd);
 237
 238         return 0;
 239 }
 240
 241 int po1030_init_controls(struct sd *sd)
 242 {
 243         struct v4l2_ctrl_handler *hdl = &sd->gspca_dev.ctrl_handler;
 244
 245         sd->gspca_dev.vdev.ctrl_handler = hdl;
 246         v4l2_ctrl_handler_init(hdl, 9);
 247
 248         sd->auto_white_bal = v4l2_ctrl_new_std(hdl, &po1030_ctrl_ops,
 249                                                V4L2_CID_AUTO_WHITE_BALANCE,
 250                                                0, 1, 1, 0);
 251         sd->green_bal = v4l2_ctrl_new_custom(hdl, &po1030_greenbal_cfg, NULL);
 252         sd->red_bal = v4l2_ctrl_new_std(hdl, &po1030_ctrl_ops,
 253                                         V4L2_CID_RED_BALANCE, 0, 255, 1,
 254                                         PO1030_RED_GAIN_DEFAULT);
 255         sd->blue_bal = v4l2_ctrl_new_std(hdl, &po1030_ctrl_ops,
 256                                         V4L2_CID_BLUE_BALANCE, 0, 255, 1,
 257                                         PO1030_BLUE_GAIN_DEFAULT);
 258
 259         sd->autoexpo = v4l2_ctrl_new_std_menu(hdl, &po1030_ctrl_ops,
 260                           V4L2_CID_EXPOSURE_AUTO, 1, 0, V4L2_EXPOSURE_MANUAL);
 261         sd->expo = v4l2_ctrl_new_std(hdl, &po1030_ctrl_ops, V4L2_CID_EXPOSURE,
 262                           0, 0x2ff, 1, PO1030_EXPOSURE_DEFAULT);
 263
 264         sd->gain = v4l2_ctrl_new_std(hdl, &po1030_ctrl_ops, V4L2_CID_GAIN, 0,
 265                                      0x4f, 1, PO1030_GLOBAL_GAIN_DEFAULT);
 266
 267         sd->hflip = v4l2_ctrl_new_std(hdl, &po1030_ctrl_ops, V4L2_CID_HFLIP,
 268                                       0, 1, 1, 0);
 269         sd->vflip = v4l2_ctrl_new_std(hdl, &po1030_ctrl_ops, V4L2_CID_VFLIP,
 270                                       0, 1, 1, 0);
 271
 272         if (hdl->error) {
 273                 pr_err("Could not initialize controls\n");
 274                 return hdl->error;
 275         }
 276
 277         v4l2_ctrl_auto_cluster(4, &sd->auto_white_bal, 0, false);
 278         v4l2_ctrl_auto_cluster(2, &sd->autoexpo, 0, false);
 279         v4l2_ctrl_cluster(2, &sd->hflip);
 280
 281         return 0;
 282 }
 283
 284 int po1030_start(struct sd *sd)
 285 {
 286         struct cam *cam = &sd->gspca_dev.cam;
 287         int i, err = 0;
 288         int width = cam->cam_mode[sd->gspca_dev.curr_mode].width;
 289         int height = cam->cam_mode[sd->gspca_dev.curr_mode].height;
 290         int ver_offs = cam->cam_mode[sd->gspca_dev.curr_mode].priv;
 291         u8 data;
 292
 293         switch (width) {
 294         case 320:
 295                 data = PO1030_SUBSAMPLING;
 296                 err = m5602_write_sensor(sd, PO1030_CONTROL3, &data, 1);
 297                 if (err < 0)
 298                         return err;
 299
 300                 data = ((width + 3) >> 8) & 0xff;
 301                 err = m5602_write_sensor(sd, PO1030_WINDOWWIDTH_H, &data, 1);
 302                 if (err < 0)
 303                         return err;
 304
 305                 data = (width + 3) & 0xff;
 306                 err = m5602_write_sensor(sd, PO1030_WINDOWWIDTH_L, &data, 1);
 307                 if (err < 0)
 308                         return err;
 309
 310                 data = ((height + 1) >> 8) & 0xff;
 311                 err = m5602_write_sensor(sd, PO1030_WINDOWHEIGHT_H, &data, 1);
 312                 if (err < 0)
 313                         return err;
 314
 315                 data = (height + 1) & 0xff;
 316                 err = m5602_write_sensor(sd, PO1030_WINDOWHEIGHT_L, &data, 1);
 317
 318                 height += 6;
 319                 width -= 1;
 320                 break;
 321
 322         case 640:
 323                 data = 0;
 324                 err = m5602_write_sensor(sd, PO1030_CONTROL3, &data, 1);
 325                 if (err < 0)
 326                         return err;
 327
 328                 data = ((width + 7) >> 8) & 0xff;
 329                 err = m5602_write_sensor(sd, PO1030_WINDOWWIDTH_H, &data, 1);
 330                 if (err < 0)
 331                         return err;
 332
 333                 data = (width + 7) & 0xff;
 334                 err = m5602_write_sensor(sd, PO1030_WINDOWWIDTH_L, &data, 1);
 335                 if (err < 0)
 336                         return err;
 337
 338                 data = ((height + 3) >> 8) & 0xff;
 339                 err = m5602_write_sensor(sd, PO1030_WINDOWHEIGHT_H, &data, 1);
 340                 if (err < 0)
 341                         return err;
 342
 343                 data = (height + 3) & 0xff;
 344                 err = m5602_write_sensor(sd, PO1030_WINDOWHEIGHT_L, &data, 1);
 345
 346                 height += 12;
 347                 width -= 2;
 348                 break;
 349         }
 350         err = m5602_write_bridge(sd, M5602_XB_SENSOR_TYPE, 0x0c);
 351         if (err < 0)
 352                 return err;
 353
 354         err = m5602_write_bridge(sd, M5602_XB_LINE_OF_FRAME_H, 0x81);
 355         if (err < 0)
 356                 return err;
 357
 358         err = m5602_write_bridge(sd, M5602_XB_PIX_OF_LINE_H, 0x82);
 359         if (err < 0)
 360                 return err;
 361
 362         err = m5602_write_bridge(sd, M5602_XB_SIG_INI, 0x01);
 363         if (err < 0)
 364                 return err;
 365
 366         err = m5602_write_bridge(sd, M5602_XB_VSYNC_PARA,
 367                                  ((ver_offs >> 8) & 0xff));
 368         if (err < 0)
 369                 return err;
 370
 371         err = m5602_write_bridge(sd, M5602_XB_VSYNC_PARA, (ver_offs & 0xff));
 372         if (err < 0)
 373                 return err;
 374
 375         for (i = 0; i < 2 && !err; i++)
 376                 err = m5602_write_bridge(sd, M5602_XB_VSYNC_PARA, 0);
 377         if (err < 0)
 378                 return err;
 379
 380         err = m5602_write_bridge(sd, M5602_XB_VSYNC_PARA, (height >> 8) & 0xff);
 381         if (err < 0)
 382                 return err;
 383
 384         err = m5602_write_bridge(sd, M5602_XB_VSYNC_PARA, (height & 0xff));
 385         if (err < 0)
 386                 return err;
 387
 388         for (i = 0; i < 2 && !err; i++)
 389                 err = m5602_write_bridge(sd, M5602_XB_VSYNC_PARA, 0);
 390
 391         for (i = 0; i < 2 && !err; i++)
 392                 err = m5602_write_bridge(sd, M5602_XB_SIG_INI, 0);
 393
 394         for (i = 0; i < 2 && !err; i++)
 395                 err = m5602_write_bridge(sd, M5602_XB_HSYNC_PARA, 0);
 396         if (err < 0)
 397                 return err;
 398
 399         err = m5602_write_bridge(sd, M5602_XB_HSYNC_PARA, (width >> 8) & 0xff);
 400         if (err < 0)
 401                 return err;
 402
 403         err = m5602_write_bridge(sd, M5602_XB_HSYNC_PARA, (width & 0xff));
 404         if (err < 0)
 405                 return err;
 406
 407         err = m5602_write_bridge(sd, M5602_XB_SIG_INI, 0);
 408         return err;
 409 }
 410
 411 static int po1030_set_exposure(struct gspca_dev *gspca_dev, __s32 val)
 412 {
 413         struct sd *sd = (struct sd *) gspca_dev;
 414         u8 i2c_data;
 415         int err;
 416
 417         gspca_dbg(gspca_dev, D_CONF, "Set exposure to %d\n", val & 0xffff);
 418
 419         i2c_data = ((val & 0xff00) >> 8);
 420         gspca_dbg(gspca_dev, D_CONF, "Set exposure to high byte to 0x%x\n",
 421                   i2c_data);
 422
 423         err = m5602_write_sensor(sd, PO1030_INTEGLINES_H,
 424                                   &i2c_data, 1);
 425         if (err < 0)
 426                 return err;
 427
 428         i2c_data = (val & 0xff);
 429         gspca_dbg(gspca_dev, D_CONF, "Set exposure to low byte to 0x%x\n",
 430                   i2c_data);
 431         err = m5602_write_sensor(sd, PO1030_INTEGLINES_M,
 432                                   &i2c_data, 1);
 433
 434         return err;
 435 }
 436
 437 static int po1030_set_gain(struct gspca_dev *gspca_dev, __s32 val)
 438 {
 439         struct sd *sd = (struct sd *) gspca_dev;
 440         u8 i2c_data;
 441         int err;
 442
 443         i2c_data = val & 0xff;
 444         gspca_dbg(gspca_dev, D_CONF, "Set global gain to %d\n", i2c_data);
 445         err = m5602_write_sensor(sd, PO1030_GLOBALGAIN,
 446                                  &i2c_data, 1);
 447         return err;
 448 }
 449
 450 static int po1030_set_hvflip(struct gspca_dev *gspca_dev)
 451 {
 452         struct sd *sd = (struct sd *) gspca_dev;
 453         u8 i2c_data;
 454         int err;
 455
 456         gspca_dbg(gspca_dev, D_CONF, "Set hvflip %d %d\n",
 457                   sd->hflip->val, sd->vflip->val);
 458         err = m5602_read_sensor(sd, PO1030_CONTROL2, &i2c_data, 1);
 459         if (err < 0)
 460                 return err;
 461
 462         i2c_data = (0x3f & i2c_data) | (sd->hflip->val << 7) |
 463                    (sd->vflip->val << 6);
 464
 465         err = m5602_write_sensor(sd, PO1030_CONTROL2,
 466                                  &i2c_data, 1);
 467
 468         return err;
 469 }
 470
 471 static int po1030_set_red_balance(struct gspca_dev *gspca_dev, __s32 val)
 472 {
 473         struct sd *sd = (struct sd *) gspca_dev;
 474         u8 i2c_data;
 475         int err;
 476
 477         i2c_data = val & 0xff;
 478         gspca_dbg(gspca_dev, D_CONF, "Set red gain to %d\n", i2c_data);
 479         err = m5602_write_sensor(sd, PO1030_RED_GAIN,
 480                                   &i2c_data, 1);
 481         return err;
 482 }
 483
 484 static int po1030_set_blue_balance(struct gspca_dev *gspca_dev, __s32 val)
 485 {
 486         struct sd *sd = (struct sd *) gspca_dev;
 487         u8 i2c_data;
 488         int err;
 489
 490         i2c_data = val & 0xff;
 491         gspca_dbg(gspca_dev, D_CONF, "Set blue gain to %d\n", i2c_data);
 492         err = m5602_write_sensor(sd, PO1030_BLUE_GAIN,
 493                                   &i2c_data, 1);
 494
 495         return err;
 496 }
 497
 498 static int po1030_set_green_balance(struct gspca_dev *gspca_dev, __s32 val)
 499 {
 500         struct sd *sd = (struct sd *) gspca_dev;
 501         u8 i2c_data;
 502         int err;
 503
 504         i2c_data = val & 0xff;
 505         gspca_dbg(gspca_dev, D_CONF, "Set green gain to %d\n", i2c_data);
 506
 507         err = m5602_write_sensor(sd, PO1030_GREEN_1_GAIN,
 508                            &i2c_data, 1);
 509         if (err < 0)
 510                 return err;
 511
 512         return m5602_write_sensor(sd, PO1030_GREEN_2_GAIN,
 513                                  &i2c_data, 1);
 514 }
 515
 516 static int po1030_set_auto_white_balance(struct gspca_dev *gspca_dev,
 517                                          __s32 val)
 518 {
 519         struct sd *sd = (struct sd *) gspca_dev;
 520         u8 i2c_data;
 521         int err;
 522
 523         err = m5602_read_sensor(sd, PO1030_AUTOCTRL1, &i2c_data, 1);
 524         if (err < 0)
 525                 return err;
 526
 527         gspca_dbg(gspca_dev, D_CONF, "Set auto white balance to %d\n", val);
 528         i2c_data = (i2c_data & 0xfe) | (val & 0x01);
 529         err = m5602_write_sensor(sd, PO1030_AUTOCTRL1, &i2c_data, 1);
 530         return err;
 531 }
 532
 533 static int po1030_set_auto_exposure(struct gspca_dev *gspca_dev,
 534                                     __s32 val)
 535 {
 536         struct sd *sd = (struct sd *) gspca_dev;
 537         u8 i2c_data;
 538         int err;
 539
 540         err = m5602_read_sensor(sd, PO1030_AUTOCTRL1, &i2c_data, 1);
 541         if (err < 0)
 542                 return err;
 543
 544         gspca_dbg(gspca_dev, D_CONF, "Set auto exposure to %d\n", val);
 545         val = (val == V4L2_EXPOSURE_AUTO);
 546         i2c_data = (i2c_data & 0xfd) | ((val & 0x01) << 1);
 547         return m5602_write_sensor(sd, PO1030_AUTOCTRL1, &i2c_data, 1);
 548 }
 549
 550 void po1030_disconnect(struct sd *sd)
 551 {
 552         sd->sensor = NULL;
 553 }
 554
 555 static int po1030_s_ctrl(struct v4l2_ctrl *ctrl)
 556 {
 557         struct gspca_dev *gspca_dev =
 558                 container_of(ctrl->handler, struct gspca_dev, ctrl_handler);
 559         struct sd *sd = (struct sd *) gspca_dev;
 560         int err;
 561
 562         if (!gspca_dev->streaming)
 563                 return 0;
 564
 565         switch (ctrl->id) {
 566         case V4L2_CID_AUTO_WHITE_BALANCE:
 567                 err = po1030_set_auto_white_balance(gspca_dev, ctrl->val);
 568                 if (err || ctrl->val)
 569                         return err;
 570                 err = po1030_set_green_balance(gspca_dev, sd->green_bal->val);
 571                 if (err)
 572                         return err;
 573                 err = po1030_set_red_balance(gspca_dev, sd->red_bal->val);
 574                 if (err)
 575                         return err;
 576                 err = po1030_set_blue_balance(gspca_dev, sd->blue_bal->val);
 577                 break;
 578         case V4L2_CID_EXPOSURE_AUTO:
 579                 err = po1030_set_auto_exposure(gspca_dev, ctrl->val);
 580                 if (err || ctrl->val == V4L2_EXPOSURE_AUTO)
 581                         return err;
 582                 err = po1030_set_exposure(gspca_dev, sd->expo->val);
 583                 break;
 584         case V4L2_CID_GAIN:
 585                 err = po1030_set_gain(gspca_dev, ctrl->val);
 586                 break;
 587         case V4L2_CID_HFLIP:
 588                 err = po1030_set_hvflip(gspca_dev);
 589                 break;
 590         default:
 591                 return -EINVAL;
 592         }
 593
 594         return err;
 595 }
 596
 597 static void po1030_dump_registers(struct sd *sd)
 598 {
 599         int address;
 600         u8 value = 0;
 601
 602         pr_info("Dumping the po1030 sensor core registers\n");
 603         for (address = 0; address < 0x7f; address++) {
 604                 m5602_read_sensor(sd, address, &value, 1);
 605                 pr_info("register 0x%x contains 0x%x\n", address, value);
 606         }
 607
 608         pr_info("po1030 register state dump complete\n");
 609
 610         pr_info("Probing for which registers that are read/write\n");
 611         for (address = 0; address < 0xff; address++) {
 612                 u8 old_value, ctrl_value;
 613                 u8 test_value[2] = {0xff, 0xff};
 614
 615                 m5602_read_sensor(sd, address, &old_value, 1);
 616                 m5602_write_sensor(sd, address, test_value, 1);
 617                 m5602_read_sensor(sd, address, &ctrl_value, 1);
 618
 619                 if (ctrl_value == test_value[0])
 620                         pr_info("register 0x%x is writeable\n", address);
 621                 else
 622                         pr_info("register 0x%x is read only\n", address);
 623
 624                 /* Restore original value */
 625                 m5602_write_sensor(sd, address, &old_value, 1);
 626         }
 627 }