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         struct gspca_dev *gspca_dev = (struct gspca_dev *)sd;
 163
 164         if (force_sensor) {
 165                 if (force_sensor == PO1030_SENSOR) {
 166                         pr_info("Forcing a %s sensor\n", po1030.name);
 167                         goto sensor_found;
 168                 }
 169                 /* If we want to force another sensor, don't try to probe this
 170                  * one */
 171                 return -ENODEV;
 172         }
 173
 174         PDEBUG(D_PROBE, "Probing for a po1030 sensor");
 175
 176         /* Run the pre-init to actually probe the unit */
 177         for (i = 0; i < ARRAY_SIZE(preinit_po1030); i++) {
 178                 u8 data = preinit_po1030[i][2];
 179                 if (preinit_po1030[i][0] == SENSOR)
 180                         m5602_write_sensor(sd,
 181                                 preinit_po1030[i][1], &data, 1);
 182                 else
 183                         m5602_write_bridge(sd, preinit_po1030[i][1], data);
 184         }
 185
 186         if (m5602_read_sensor(sd, PO1030_DEVID_H, &dev_id_h, 1))
 187                 return -ENODEV;
 188
 189         if (dev_id_h == 0x30) {
 190                 pr_info("Detected a po1030 sensor\n");
 191                 goto sensor_found;
 192         }
 193         return -ENODEV;
 194
 195 sensor_found:
 196         sd->gspca_dev.cam.cam_mode = po1030_modes;
 197         sd->gspca_dev.cam.nmodes = ARRAY_SIZE(po1030_modes);
 198
 199         return 0;
 200 }
 201
 202 int po1030_init(struct sd *sd)
 203 {
 204         int i, err = 0;
 205
 206         /* Init the sensor */
 207         for (i = 0; i < ARRAY_SIZE(init_po1030) && !err; i++) {
 208                 u8 data[2] = {0x00, 0x00};
 209
 210                 switch (init_po1030[i][0]) {
 211                 case BRIDGE:
 212                         err = m5602_write_bridge(sd,
 213                                 init_po1030[i][1],
 214                                 init_po1030[i][2]);
 215                         break;
 216
 217                 case SENSOR:
 218                         data[0] = init_po1030[i][2];
 219                         err = m5602_write_sensor(sd,
 220                                 init_po1030[i][1], data, 1);
 221                         break;
 222
 223                 default:
 224                         pr_info("Invalid stream command, exiting init\n");
 225                         return -EINVAL;
 226                 }
 227         }
 228         if (err < 0)
 229                 return err;
 230
 231         if (dump_sensor)
 232                 po1030_dump_registers(sd);
 233
 234         return 0;
 235 }
 236
 237 int po1030_init_controls(struct sd *sd)
 238 {
 239         struct v4l2_ctrl_handler *hdl = &sd->gspca_dev.ctrl_handler;
 240
 241         sd->gspca_dev.vdev.ctrl_handler = hdl;
 242         v4l2_ctrl_handler_init(hdl, 9);
 243
 244         sd->auto_white_bal = v4l2_ctrl_new_std(hdl, &po1030_ctrl_ops,
 245                                                V4L2_CID_AUTO_WHITE_BALANCE,
 246                                                0, 1, 1, 0);
 247         sd->green_bal = v4l2_ctrl_new_custom(hdl, &po1030_greenbal_cfg, NULL);
 248         sd->red_bal = v4l2_ctrl_new_std(hdl, &po1030_ctrl_ops,
 249                                         V4L2_CID_RED_BALANCE, 0, 255, 1,
 250                                         PO1030_RED_GAIN_DEFAULT);
 251         sd->blue_bal = v4l2_ctrl_new_std(hdl, &po1030_ctrl_ops,
 252                                         V4L2_CID_BLUE_BALANCE, 0, 255, 1,
 253                                         PO1030_BLUE_GAIN_DEFAULT);
 254
 255         sd->autoexpo = v4l2_ctrl_new_std_menu(hdl, &po1030_ctrl_ops,
 256                           V4L2_CID_EXPOSURE_AUTO, 1, 0, V4L2_EXPOSURE_MANUAL);
 257         sd->expo = v4l2_ctrl_new_std(hdl, &po1030_ctrl_ops, V4L2_CID_EXPOSURE,
 258                           0, 0x2ff, 1, PO1030_EXPOSURE_DEFAULT);
 259
 260         sd->gain = v4l2_ctrl_new_std(hdl, &po1030_ctrl_ops, V4L2_CID_GAIN, 0,
 261                                      0x4f, 1, PO1030_GLOBAL_GAIN_DEFAULT);
 262
 263         sd->hflip = v4l2_ctrl_new_std(hdl, &po1030_ctrl_ops, V4L2_CID_HFLIP,
 264                                       0, 1, 1, 0);
 265         sd->vflip = v4l2_ctrl_new_std(hdl, &po1030_ctrl_ops, V4L2_CID_VFLIP,
 266                                       0, 1, 1, 0);
 267
 268         if (hdl->error) {
 269                 pr_err("Could not initialize controls\n");
 270                 return hdl->error;
 271         }
 272
 273         v4l2_ctrl_auto_cluster(4, &sd->auto_white_bal, 0, false);
 274         v4l2_ctrl_auto_cluster(2, &sd->autoexpo, 0, false);
 275         v4l2_ctrl_cluster(2, &sd->hflip);
 276
 277         return 0;
 278 }
 279
 280 int po1030_start(struct sd *sd)
 281 {
 282         struct cam *cam = &sd->gspca_dev.cam;
 283         int i, err = 0;
 284         int width = cam->cam_mode[sd->gspca_dev.curr_mode].width;
 285         int height = cam->cam_mode[sd->gspca_dev.curr_mode].height;
 286         int ver_offs = cam->cam_mode[sd->gspca_dev.curr_mode].priv;
 287         u8 data;
 288
 289         switch (width) {
 290         case 320:
 291                 data = PO1030_SUBSAMPLING;
 292                 err = m5602_write_sensor(sd, PO1030_CONTROL3, &data, 1);
 293                 if (err < 0)
 294                         return err;
 295
 296                 data = ((width + 3) >> 8) & 0xff;
 297                 err = m5602_write_sensor(sd, PO1030_WINDOWWIDTH_H, &data, 1);
 298                 if (err < 0)
 299                         return err;
 300
 301                 data = (width + 3) & 0xff;
 302                 err = m5602_write_sensor(sd, PO1030_WINDOWWIDTH_L, &data, 1);
 303                 if (err < 0)
 304                         return err;
 305
 306                 data = ((height + 1) >> 8) & 0xff;
 307                 err = m5602_write_sensor(sd, PO1030_WINDOWHEIGHT_H, &data, 1);
 308                 if (err < 0)
 309                         return err;
 310
 311                 data = (height + 1) & 0xff;
 312                 err = m5602_write_sensor(sd, PO1030_WINDOWHEIGHT_L, &data, 1);
 313
 314                 height += 6;
 315                 width -= 1;
 316                 break;
 317
 318         case 640:
 319                 data = 0;
 320                 err = m5602_write_sensor(sd, PO1030_CONTROL3, &data, 1);
 321                 if (err < 0)
 322                         return err;
 323
 324                 data = ((width + 7) >> 8) & 0xff;
 325                 err = m5602_write_sensor(sd, PO1030_WINDOWWIDTH_H, &data, 1);
 326                 if (err < 0)
 327                         return err;
 328
 329                 data = (width + 7) & 0xff;
 330                 err = m5602_write_sensor(sd, PO1030_WINDOWWIDTH_L, &data, 1);
 331                 if (err < 0)
 332                         return err;
 333
 334                 data = ((height + 3) >> 8) & 0xff;
 335                 err = m5602_write_sensor(sd, PO1030_WINDOWHEIGHT_H, &data, 1);
 336                 if (err < 0)
 337                         return err;
 338
 339                 data = (height + 3) & 0xff;
 340                 err = m5602_write_sensor(sd, PO1030_WINDOWHEIGHT_L, &data, 1);
 341
 342                 height += 12;
 343                 width -= 2;
 344                 break;
 345         }
 346         err = m5602_write_bridge(sd, M5602_XB_SENSOR_TYPE, 0x0c);
 347         if (err < 0)
 348                 return err;
 349
 350         err = m5602_write_bridge(sd, M5602_XB_LINE_OF_FRAME_H, 0x81);
 351         if (err < 0)
 352                 return err;
 353
 354         err = m5602_write_bridge(sd, M5602_XB_PIX_OF_LINE_H, 0x82);
 355         if (err < 0)
 356                 return err;
 357
 358         err = m5602_write_bridge(sd, M5602_XB_SIG_INI, 0x01);
 359         if (err < 0)
 360                 return err;
 361
 362         err = m5602_write_bridge(sd, M5602_XB_VSYNC_PARA,
 363                                  ((ver_offs >> 8) & 0xff));
 364         if (err < 0)
 365                 return err;
 366
 367         err = m5602_write_bridge(sd, M5602_XB_VSYNC_PARA, (ver_offs & 0xff));
 368         if (err < 0)
 369                 return err;
 370
 371         for (i = 0; i < 2 && !err; i++)
 372                 err = m5602_write_bridge(sd, M5602_XB_VSYNC_PARA, 0);
 373         if (err < 0)
 374                 return err;
 375
 376         err = m5602_write_bridge(sd, M5602_XB_VSYNC_PARA, (height >> 8) & 0xff);
 377         if (err < 0)
 378                 return err;
 379
 380         err = m5602_write_bridge(sd, M5602_XB_VSYNC_PARA, (height & 0xff));
 381         if (err < 0)
 382                 return err;
 383
 384         for (i = 0; i < 2 && !err; i++)
 385                 err = m5602_write_bridge(sd, M5602_XB_VSYNC_PARA, 0);
 386
 387         for (i = 0; i < 2 && !err; i++)
 388                 err = m5602_write_bridge(sd, M5602_XB_SIG_INI, 0);
 389
 390         for (i = 0; i < 2 && !err; i++)
 391                 err = m5602_write_bridge(sd, M5602_XB_HSYNC_PARA, 0);
 392         if (err < 0)
 393                 return err;
 394
 395         err = m5602_write_bridge(sd, M5602_XB_HSYNC_PARA, (width >> 8) & 0xff);
 396         if (err < 0)
 397                 return err;
 398
 399         err = m5602_write_bridge(sd, M5602_XB_HSYNC_PARA, (width & 0xff));
 400         if (err < 0)
 401                 return err;
 402
 403         err = m5602_write_bridge(sd, M5602_XB_SIG_INI, 0);
 404         return err;
 405 }
 406
 407 static int po1030_set_exposure(struct gspca_dev *gspca_dev, __s32 val)
 408 {
 409         struct sd *sd = (struct sd *) gspca_dev;
 410         u8 i2c_data;
 411         int err;
 412
 413         PDEBUG(D_CONF, "Set exposure to %d", val & 0xffff);
 414
 415         i2c_data = ((val & 0xff00) >> 8);
 416         PDEBUG(D_CONF, "Set exposure to high byte to 0x%x",
 417                i2c_data);
 418
 419         err = m5602_write_sensor(sd, PO1030_INTEGLINES_H,
 420                                   &i2c_data, 1);
 421         if (err < 0)
 422                 return err;
 423
 424         i2c_data = (val & 0xff);
 425         PDEBUG(D_CONF, "Set exposure to low byte to 0x%x",
 426                i2c_data);
 427         err = m5602_write_sensor(sd, PO1030_INTEGLINES_M,
 428                                   &i2c_data, 1);
 429
 430         return err;
 431 }
 432
 433 static int po1030_set_gain(struct gspca_dev *gspca_dev, __s32 val)
 434 {
 435         struct sd *sd = (struct sd *) gspca_dev;
 436         u8 i2c_data;
 437         int err;
 438
 439         i2c_data = val & 0xff;
 440         PDEBUG(D_CONF, "Set global gain to %d", i2c_data);
 441         err = m5602_write_sensor(sd, PO1030_GLOBALGAIN,
 442                                  &i2c_data, 1);
 443         return err;
 444 }
 445
 446 static int po1030_set_hvflip(struct gspca_dev *gspca_dev)
 447 {
 448         struct sd *sd = (struct sd *) gspca_dev;
 449         u8 i2c_data;
 450         int err;
 451
 452         PDEBUG(D_CONF, "Set hvflip %d %d", sd->hflip->val, sd->vflip->val);
 453         err = m5602_read_sensor(sd, PO1030_CONTROL2, &i2c_data, 1);
 454         if (err < 0)
 455                 return err;
 456
 457         i2c_data = (0x3f & i2c_data) | (sd->hflip->val << 7) |
 458                    (sd->vflip->val << 6);
 459
 460         err = m5602_write_sensor(sd, PO1030_CONTROL2,
 461                                  &i2c_data, 1);
 462
 463         return err;
 464 }
 465
 466 static int po1030_set_red_balance(struct gspca_dev *gspca_dev, __s32 val)
 467 {
 468         struct sd *sd = (struct sd *) gspca_dev;
 469         u8 i2c_data;
 470         int err;
 471
 472         i2c_data = val & 0xff;
 473         PDEBUG(D_CONF, "Set red gain to %d", i2c_data);
 474         err = m5602_write_sensor(sd, PO1030_RED_GAIN,
 475                                   &i2c_data, 1);
 476         return err;
 477 }
 478
 479 static int po1030_set_blue_balance(struct gspca_dev *gspca_dev, __s32 val)
 480 {
 481         struct sd *sd = (struct sd *) gspca_dev;
 482         u8 i2c_data;
 483         int err;
 484
 485         i2c_data = val & 0xff;
 486         PDEBUG(D_CONF, "Set blue gain to %d", i2c_data);
 487         err = m5602_write_sensor(sd, PO1030_BLUE_GAIN,
 488                                   &i2c_data, 1);
 489
 490         return err;
 491 }
 492
 493 static int po1030_set_green_balance(struct gspca_dev *gspca_dev, __s32 val)
 494 {
 495         struct sd *sd = (struct sd *) gspca_dev;
 496         u8 i2c_data;
 497         int err;
 498
 499         i2c_data = val & 0xff;
 500         PDEBUG(D_CONF, "Set green gain to %d", i2c_data);
 501
 502         err = m5602_write_sensor(sd, PO1030_GREEN_1_GAIN,
 503                            &i2c_data, 1);
 504         if (err < 0)
 505                 return err;
 506
 507         return m5602_write_sensor(sd, PO1030_GREEN_2_GAIN,
 508                                  &i2c_data, 1);
 509 }
 510
 511 static int po1030_set_auto_white_balance(struct gspca_dev *gspca_dev,
 512                                          __s32 val)
 513 {
 514         struct sd *sd = (struct sd *) gspca_dev;
 515         u8 i2c_data;
 516         int err;
 517
 518         err = m5602_read_sensor(sd, PO1030_AUTOCTRL1, &i2c_data, 1);
 519         if (err < 0)
 520                 return err;
 521
 522         PDEBUG(D_CONF, "Set auto white balance to %d", val);
 523         i2c_data = (i2c_data & 0xfe) | (val & 0x01);
 524         err = m5602_write_sensor(sd, PO1030_AUTOCTRL1, &i2c_data, 1);
 525         return err;
 526 }
 527
 528 static int po1030_set_auto_exposure(struct gspca_dev *gspca_dev,
 529                                     __s32 val)
 530 {
 531         struct sd *sd = (struct sd *) gspca_dev;
 532         u8 i2c_data;
 533         int err;
 534
 535         err = m5602_read_sensor(sd, PO1030_AUTOCTRL1, &i2c_data, 1);
 536         if (err < 0)
 537                 return err;
 538
 539         PDEBUG(D_CONF, "Set auto exposure to %d", val);
 540         val = (val == V4L2_EXPOSURE_AUTO);
 541         i2c_data = (i2c_data & 0xfd) | ((val & 0x01) << 1);
 542         return m5602_write_sensor(sd, PO1030_AUTOCTRL1, &i2c_data, 1);
 543 }
 544
 545 void po1030_disconnect(struct sd *sd)
 546 {
 547         sd->sensor = NULL;
 548 }
 549
 550 static int po1030_s_ctrl(struct v4l2_ctrl *ctrl)
 551 {
 552         struct gspca_dev *gspca_dev =
 553                 container_of(ctrl->handler, struct gspca_dev, ctrl_handler);
 554         struct sd *sd = (struct sd *) gspca_dev;
 555         int err;
 556
 557         if (!gspca_dev->streaming)
 558                 return 0;
 559
 560         switch (ctrl->id) {
 561         case V4L2_CID_AUTO_WHITE_BALANCE:
 562                 err = po1030_set_auto_white_balance(gspca_dev, ctrl->val);
 563                 if (err || ctrl->val)
 564                         return err;
 565                 err = po1030_set_green_balance(gspca_dev, sd->green_bal->val);
 566                 if (err)
 567                         return err;
 568                 err = po1030_set_red_balance(gspca_dev, sd->red_bal->val);
 569                 if (err)
 570                         return err;
 571                 err = po1030_set_blue_balance(gspca_dev, sd->blue_bal->val);
 572                 break;
 573         case V4L2_CID_EXPOSURE_AUTO:
 574                 err = po1030_set_auto_exposure(gspca_dev, ctrl->val);
 575                 if (err || ctrl->val == V4L2_EXPOSURE_AUTO)
 576                         return err;
 577                 err = po1030_set_exposure(gspca_dev, sd->expo->val);
 578                 break;
 579         case V4L2_CID_GAIN:
 580                 err = po1030_set_gain(gspca_dev, ctrl->val);
 581                 break;
 582         case V4L2_CID_HFLIP:
 583                 err = po1030_set_hvflip(gspca_dev);
 584                 break;
 585         default:
 586                 return -EINVAL;
 587         }
 588
 589         return err;
 590 }
 591
 592 static void po1030_dump_registers(struct sd *sd)
 593 {
 594         int address;
 595         u8 value = 0;
 596
 597         pr_info("Dumping the po1030 sensor core registers\n");
 598         for (address = 0; address < 0x7f; address++) {
 599                 m5602_read_sensor(sd, address, &value, 1);
 600                 pr_info("register 0x%x contains 0x%x\n", address, value);
 601         }
 602
 603         pr_info("po1030 register state dump complete\n");
 604
 605         pr_info("Probing for which registers that are read/write\n");
 606         for (address = 0; address < 0xff; address++) {
 607                 u8 old_value, ctrl_value;
 608                 u8 test_value[2] = {0xff, 0xff};
 609
 610                 m5602_read_sensor(sd, address, &old_value, 1);
 611                 m5602_write_sensor(sd, address, test_value, 1);
 612                 m5602_read_sensor(sd, address, &ctrl_value, 1);
 613
 614                 if (ctrl_value == test_value[0])
 615                         pr_info("register 0x%x is writeable\n", address);
 616                 else
 617                         pr_info("register 0x%x is read only\n", address);
 618
 619                 /* Restore original value */
 620                 m5602_write_sensor(sd, address, &old_value, 1);
 621         }
 622 }