SAFE public projects git trees. - safe/jmp/linux-2.6/blob - drivers/media/video/gspca/m5602/m5602_s5k4aa.c

   1 /*
   2  * Driver for the s5k4aa 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 #include "m5602_s5k4aa.h"
  20
  21 static int s5k4aa_get_exposure(struct gspca_dev *gspca_dev, __s32 *val);
  22 static int s5k4aa_set_exposure(struct gspca_dev *gspca_dev, __s32 val);
  23 static int s5k4aa_get_vflip(struct gspca_dev *gspca_dev, __s32 *val);
  24 static int s5k4aa_set_vflip(struct gspca_dev *gspca_dev, __s32 val);
  25 static int s5k4aa_get_hflip(struct gspca_dev *gspca_dev, __s32 *val);
  26 static int s5k4aa_set_hflip(struct gspca_dev *gspca_dev, __s32 val);
  27 static int s5k4aa_get_gain(struct gspca_dev *gspca_dev, __s32 *val);
  28 static int s5k4aa_set_gain(struct gspca_dev *gspca_dev, __s32 val);
  29 static int s5k4aa_get_noise(struct gspca_dev *gspca_dev, __s32 *val);
  30 static int s5k4aa_set_noise(struct gspca_dev *gspca_dev, __s32 val);
  31 static int s5k4aa_get_brightness(struct gspca_dev *gspca_dev, __s32 *val);
  32 static int s5k4aa_set_brightness(struct gspca_dev *gspca_dev, __s32 val);
  33
  34 static
  35     const
  36         struct dmi_system_id s5k4aa_vflip_dmi_table[] = {
  37         {
  38                 .ident = "Fujitsu-Siemens Amilo Xa 2528",
  39                 .matches = {
  40                         DMI_MATCH(DMI_SYS_VENDOR, "FUJITSU SIEMENS"),
  41                         DMI_MATCH(DMI_PRODUCT_NAME, "AMILO Xa 2528")
  42                 }
  43         }, {
  44                 .ident = "Fujitsu-Siemens Amilo Xi 2550",
  45                 .matches = {
  46                         DMI_MATCH(DMI_SYS_VENDOR, "FUJITSU SIEMENS"),
  47                         DMI_MATCH(DMI_PRODUCT_NAME, "AMILO Xi 2550")
  48                 }
  49         }, {
  50                 .ident = "MSI GX700",
  51                 .matches = {
  52                         DMI_MATCH(DMI_SYS_VENDOR, "Micro-Star International"),
  53                         DMI_MATCH(DMI_PRODUCT_NAME, "GX700"),
  54                         DMI_MATCH(DMI_BIOS_DATE, "07/26/2007")
  55                 }
  56         }, {
  57                 .ident = "MSI GX700/GX705/EX700",
  58                 .matches = {
  59                         DMI_MATCH(DMI_SYS_VENDOR, "Micro-Star International"),
  60                         DMI_MATCH(DMI_PRODUCT_NAME, "GX700/GX705/EX700")
  61                 }
  62         },
  63         { }
  64 };
  65
  66 static struct v4l2_pix_format s5k4aa_modes[] = {
  67         {
  68                 640,
  69                 480,
  70                 V4L2_PIX_FMT_SBGGR8,
  71                 V4L2_FIELD_NONE,
  72                 .sizeimage =
  73                         640 * 480,
  74                 .bytesperline = 640,
  75                 .colorspace = V4L2_COLORSPACE_SRGB,
  76                 .priv = 0
  77         }
  78 };
  79
  80 static const struct ctrl s5k4aa_ctrls[] = {
  81 #define VFLIP_IDX 0
  82         {
  83                 {
  84                         .id             = V4L2_CID_VFLIP,
  85                         .type           = V4L2_CTRL_TYPE_BOOLEAN,
  86                         .name           = "vertical flip",
  87                         .minimum        = 0,
  88                         .maximum        = 1,
  89                         .step           = 1,
  90                         .default_value  = 0
  91                 },
  92                 .set = s5k4aa_set_vflip,
  93                 .get = s5k4aa_get_vflip
  94         },
  95 #define HFLIP_IDX 1
  96         {
  97                 {
  98                         .id             = V4L2_CID_HFLIP,
  99                         .type           = V4L2_CTRL_TYPE_BOOLEAN,
 100                         .name           = "horizontal flip",
 101                         .minimum        = 0,
 102                         .maximum        = 1,
 103                         .step           = 1,
 104                         .default_value  = 0
 105                 },
 106                 .set = s5k4aa_set_hflip,
 107                 .get = s5k4aa_get_hflip
 108         },
 109 #define GAIN_IDX 2
 110         {
 111                 {
 112                         .id             = V4L2_CID_GAIN,
 113                         .type           = V4L2_CTRL_TYPE_INTEGER,
 114                         .name           = "Gain",
 115                         .minimum        = 0,
 116                         .maximum        = 127,
 117                         .step           = 1,
 118                         .default_value  = S5K4AA_DEFAULT_GAIN,
 119                         .flags          = V4L2_CTRL_FLAG_SLIDER
 120                 },
 121                 .set = s5k4aa_set_gain,
 122                 .get = s5k4aa_get_gain
 123         },
 124 #define EXPOSURE_IDX 3
 125         {
 126                 {
 127                         .id             = V4L2_CID_EXPOSURE,
 128                         .type           = V4L2_CTRL_TYPE_INTEGER,
 129                         .name           = "Exposure",
 130                         .minimum        = 13,
 131                         .maximum        = 0xfff,
 132                         .step           = 1,
 133                         .default_value  = 0x100,
 134                         .flags          = V4L2_CTRL_FLAG_SLIDER
 135                 },
 136                 .set = s5k4aa_set_exposure,
 137                 .get = s5k4aa_get_exposure
 138         },
 139 #define NOISE_SUPP_IDX 4
 140         {
 141                 {
 142                         .id             = V4L2_CID_PRIVATE_BASE,
 143                         .type           = V4L2_CTRL_TYPE_BOOLEAN,
 144                         .name           = "Noise suppression (smoothing)",
 145                         .minimum        = 0,
 146                         .maximum        = 1,
 147                         .step           = 1,
 148                         .default_value  = 1,
 149                 },
 150                         .set = s5k4aa_set_noise,
 151                         .get = s5k4aa_get_noise
 152         },
 153 #define BRIGHTNESS_IDX 5
 154         {
 155                 {
 156                         .id             = V4L2_CID_BRIGHTNESS,
 157                         .type           = V4L2_CTRL_TYPE_INTEGER,
 158                         .name           = "Brightness",
 159                         .minimum        = 0,
 160                         .maximum        = 0x1f,
 161                         .step           = 1,
 162                         .default_value  = S5K4AA_DEFAULT_BRIGHTNESS,
 163                 },
 164                         .set = s5k4aa_set_brightness,
 165                         .get = s5k4aa_get_brightness
 166         },
 167
 168 };
 169
 170 static void s5k4aa_dump_registers(struct sd *sd);
 171
 172 int s5k4aa_probe(struct sd *sd)
 173 {
 174         u8 prod_id[6] = {0x00, 0x00, 0x00, 0x00, 0x00, 0x00};
 175         const u8 expected_prod_id[6] = {0x00, 0x10, 0x00, 0x4b, 0x33, 0x75};
 176         int i, err = 0;
 177         s32 *sensor_settings;
 178
 179         if (force_sensor) {
 180                 if (force_sensor == S5K4AA_SENSOR) {
 181                         info("Forcing a %s sensor", s5k4aa.name);
 182                         goto sensor_found;
 183                 }
 184                 /* If we want to force another sensor, don't try to probe this
 185                  * one */
 186                 return -ENODEV;
 187         }
 188
 189         info("Probing for a s5k4aa sensor");
 190
 191         /* Preinit the sensor */
 192         for (i = 0; i < ARRAY_SIZE(preinit_s5k4aa) && !err; i++) {
 193                 u8 data[2] = {0x00, 0x00};
 194
 195                 switch (preinit_s5k4aa[i][0]) {
 196                 case BRIDGE:
 197                         err = m5602_write_bridge(sd,
 198                                                  preinit_s5k4aa[i][1],
 199                                                  preinit_s5k4aa[i][2]);
 200                         break;
 201
 202                 case SENSOR:
 203                         data[0] = preinit_s5k4aa[i][2];
 204                         err = m5602_write_sensor(sd,
 205                                                   preinit_s5k4aa[i][1],
 206                                                   data, 1);
 207                         break;
 208
 209                 case SENSOR_LONG:
 210                         data[0] = preinit_s5k4aa[i][2];
 211                         data[1] = preinit_s5k4aa[i][3];
 212                         err = m5602_write_sensor(sd,
 213                                                   preinit_s5k4aa[i][1],
 214                                                   data, 2);
 215                         break;
 216                 default:
 217                         info("Invalid stream command, exiting init");
 218                         return -EINVAL;
 219                 }
 220         }
 221
 222         /* Test some registers, but we don't know their exact meaning yet */
 223         if (m5602_read_sensor(sd, 0x00, prod_id, 2))
 224                 return -ENODEV;
 225         if (m5602_read_sensor(sd, 0x02, prod_id+2, 2))
 226                 return -ENODEV;
 227         if (m5602_read_sensor(sd, 0x04, prod_id+4, 2))
 228                 return -ENODEV;
 229
 230         if (memcmp(prod_id, expected_prod_id, sizeof(prod_id)))
 231                 return -ENODEV;
 232         else
 233                 info("Detected a s5k4aa sensor");
 234
 235 sensor_found:
 236         sensor_settings = kmalloc(
 237                 ARRAY_SIZE(s5k4aa_ctrls) * sizeof(s32), GFP_KERNEL);
 238         if (!sensor_settings)
 239                 return -ENOMEM;
 240
 241         sd->gspca_dev.cam.cam_mode = s5k4aa_modes;
 242         sd->gspca_dev.cam.nmodes = ARRAY_SIZE(s5k4aa_modes);
 243         sd->desc->ctrls = s5k4aa_ctrls;
 244         sd->desc->nctrls = ARRAY_SIZE(s5k4aa_ctrls);
 245
 246         for (i = 0; i < ARRAY_SIZE(s5k4aa_ctrls); i++)
 247                 sensor_settings[i] = s5k4aa_ctrls[i].qctrl.default_value;
 248         sd->sensor_priv = sensor_settings;
 249
 250         return 0;
 251 }
 252
 253 int s5k4aa_start(struct sd *sd)
 254 {
 255         int i, err = 0;
 256         u8 data[2];
 257         struct cam *cam = &sd->gspca_dev.cam;
 258
 259         switch (cam->cam_mode[sd->gspca_dev.curr_mode].width) {
 260         case 640:
 261                 PDEBUG(D_V4L2, "Configuring camera for VGA mode");
 262
 263                 for (i = 0; i < ARRAY_SIZE(VGA_s5k4aa); i++) {
 264                         switch (VGA_s5k4aa[i][0]) {
 265                         case BRIDGE:
 266                                 err = m5602_write_bridge(sd,
 267                                                  VGA_s5k4aa[i][1],
 268                                                  VGA_s5k4aa[i][2]);
 269                         break;
 270
 271                         case SENSOR:
 272                                 data[0] = VGA_s5k4aa[i][2];
 273                                 err = m5602_write_sensor(sd,
 274                                                  VGA_s5k4aa[i][1],
 275                                                  data, 1);
 276                         break;
 277
 278                         case SENSOR_LONG:
 279                                 data[0] = VGA_s5k4aa[i][2];
 280                                 data[1] = VGA_s5k4aa[i][3];
 281                                 err = m5602_write_sensor(sd,
 282                                                   VGA_s5k4aa[i][1],
 283                                                   data, 2);
 284                         break;
 285
 286                         default:
 287                                 err("Invalid stream command, exiting init");
 288                                 return -EINVAL;
 289                         }
 290                 }
 291         }
 292         return err;
 293 }
 294
 295 int s5k4aa_init(struct sd *sd)
 296 {
 297         int i, err = 0;
 298         s32 *sensor_settings = sd->sensor_priv;
 299
 300         for (i = 0; i < ARRAY_SIZE(init_s5k4aa) && !err; i++) {
 301                 u8 data[2] = {0x00, 0x00};
 302
 303                 switch (init_s5k4aa[i][0]) {
 304                 case BRIDGE:
 305                         err = m5602_write_bridge(sd,
 306                                 init_s5k4aa[i][1],
 307                                 init_s5k4aa[i][2]);
 308                         break;
 309
 310                 case SENSOR:
 311                         data[0] = init_s5k4aa[i][2];
 312                         err = m5602_write_sensor(sd,
 313                                 init_s5k4aa[i][1], data, 1);
 314                         break;
 315
 316                 case SENSOR_LONG:
 317                         data[0] = init_s5k4aa[i][2];
 318                         data[1] = init_s5k4aa[i][3];
 319                         err = m5602_write_sensor(sd,
 320                                 init_s5k4aa[i][1], data, 2);
 321                         break;
 322                 default:
 323                         info("Invalid stream command, exiting init");
 324                         return -EINVAL;
 325                 }
 326         }
 327
 328         if (dump_sensor)
 329                 s5k4aa_dump_registers(sd);
 330
 331         err = s5k4aa_set_exposure(&sd->gspca_dev,
 332                                    sensor_settings[EXPOSURE_IDX]);
 333         if (err < 0)
 334                 return err;
 335
 336         err = s5k4aa_set_gain(&sd->gspca_dev, sensor_settings[GAIN_IDX]);
 337         if (err < 0)
 338                 return err;
 339
 340         err = s5k4aa_set_brightness(&sd->gspca_dev,
 341                                      sensor_settings[BRIGHTNESS_IDX]);
 342         if (err < 0)
 343                 return err;
 344
 345         err = s5k4aa_set_noise(&sd->gspca_dev, sensor_settings[NOISE_SUPP_IDX]);
 346         if (err < 0)
 347                 return err;
 348
 349         err = s5k4aa_set_vflip(&sd->gspca_dev, sensor_settings[VFLIP_IDX]);
 350         if (err < 0)
 351                 return err;
 352
 353         return s5k4aa_set_hflip(&sd->gspca_dev, sensor_settings[HFLIP_IDX]);
 354 }
 355
 356 static int s5k4aa_get_exposure(struct gspca_dev *gspca_dev, __s32 *val)
 357 {
 358         struct sd *sd = (struct sd *) gspca_dev;
 359         s32 *sensor_settings = sd->sensor_priv;
 360
 361         *val = sensor_settings[EXPOSURE_IDX];
 362         PDEBUG(D_V4L2, "Read exposure %d", *val);
 363
 364         return 0;
 365 }
 366
 367 static int s5k4aa_set_exposure(struct gspca_dev *gspca_dev, __s32 val)
 368 {
 369         struct sd *sd = (struct sd *) gspca_dev;
 370         s32 *sensor_settings = sd->sensor_priv;
 371         u8 data = S5K4AA_PAGE_MAP_2;
 372         int err;
 373
 374         sensor_settings[EXPOSURE_IDX] = val;
 375         PDEBUG(D_V4L2, "Set exposure to %d", val);
 376         err = m5602_write_sensor(sd, S5K4AA_PAGE_MAP, &data, 1);
 377         if (err < 0)
 378                 return err;
 379         data = (val >> 8) & 0xff;
 380         err = m5602_write_sensor(sd, S5K4AA_EXPOSURE_HI, &data, 1);
 381         if (err < 0)
 382                 return err;
 383         data = val & 0xff;
 384         err = m5602_write_sensor(sd, S5K4AA_EXPOSURE_LO, &data, 1);
 385
 386         return err;
 387 }
 388
 389 static int s5k4aa_get_vflip(struct gspca_dev *gspca_dev, __s32 *val)
 390 {
 391         struct sd *sd = (struct sd *) gspca_dev;
 392         s32 *sensor_settings = sd->sensor_priv;
 393
 394         *val = sensor_settings[VFLIP_IDX];
 395         PDEBUG(D_V4L2, "Read vertical flip %d", *val);
 396
 397         return 0;
 398 }
 399
 400 static int s5k4aa_set_vflip(struct gspca_dev *gspca_dev, __s32 val)
 401 {
 402         struct sd *sd = (struct sd *) gspca_dev;
 403         s32 *sensor_settings = sd->sensor_priv;
 404         u8 data = S5K4AA_PAGE_MAP_2;
 405         int err;
 406
 407         sensor_settings[VFLIP_IDX] = val;
 408
 409         PDEBUG(D_V4L2, "Set vertical flip to %d", val);
 410         err = m5602_write_sensor(sd, S5K4AA_PAGE_MAP, &data, 1);
 411         if (err < 0)
 412                 return err;
 413         err = m5602_write_sensor(sd, S5K4AA_READ_MODE, &data, 1);
 414         if (err < 0)
 415                 return err;
 416
 417         err = m5602_read_sensor(sd, S5K4AA_READ_MODE, &data, 1);
 418         if (err < 0)
 419                 return err;
 420
 421         if (dmi_check_system(s5k4aa_vflip_dmi_table)) {
 422                 val = !val;
 423                 data = (data & 0x3f) |
 424                        (!sensor_settings[HFLIP_IDX] << 6) |
 425                        ((val & 0x01) << 7);
 426         } else {
 427                 data = (data & 0x3f) |
 428                 (sensor_settings[HFLIP_IDX] << 6) |
 429                 ((val & 0x01) << 7);
 430         }
 431
 432         err = m5602_write_sensor(sd, S5K4AA_READ_MODE, &data, 1);
 433         if (err < 0)
 434                 return err;
 435
 436         if (val) {
 437                 err = m5602_read_sensor(sd, S5K4AA_ROWSTART_LO, &data, 1);
 438                 if (err < 0)
 439                         return err;
 440
 441                 data++;
 442                 err = m5602_write_sensor(sd, S5K4AA_ROWSTART_LO, &data, 1);
 443         } else {
 444                 err = m5602_read_sensor(sd, S5K4AA_ROWSTART_LO, &data, 1);
 445                 if (err < 0)
 446                         return err;
 447
 448                 data--;
 449                 err = m5602_write_sensor(sd, S5K4AA_ROWSTART_LO, &data, 1);
 450         }
 451         return err;
 452 }
 453
 454 static int s5k4aa_get_hflip(struct gspca_dev *gspca_dev, __s32 *val)
 455 {
 456         struct sd *sd = (struct sd *) gspca_dev;
 457         s32 *sensor_settings = sd->sensor_priv;
 458
 459         *val = sensor_settings[HFLIP_IDX];
 460         PDEBUG(D_V4L2, "Read horizontal flip %d", *val);
 461
 462         return 0;
 463 }
 464
 465 static int s5k4aa_set_hflip(struct gspca_dev *gspca_dev, __s32 val)
 466 {
 467         struct sd *sd = (struct sd *) gspca_dev;
 468         s32 *sensor_settings = sd->sensor_priv;
 469         u8 data = S5K4AA_PAGE_MAP_2;
 470         int err;
 471
 472         sensor_settings[HFLIP_IDX] = val;
 473
 474         PDEBUG(D_V4L2, "Set horizontal flip to %d", val);
 475         err = m5602_write_sensor(sd, S5K4AA_PAGE_MAP, &data, 1);
 476         if (err < 0)
 477                 return err;
 478         err = m5602_write_sensor(sd, S5K4AA_READ_MODE, &data, 1);
 479         if (err < 0)
 480                 return err;
 481
 482         err = m5602_read_sensor(sd, S5K4AA_READ_MODE, &data, 1);
 483         if (err < 0)
 484                 return err;
 485
 486         if (dmi_check_system(s5k4aa_vflip_dmi_table)) {
 487                 val = !val;
 488                 data = (data & 0x3f) |
 489                        (!sensor_settings[VFLIP_IDX] << 7) |
 490                        ((val & 0x01) << 6);
 491         } else {
 492                 data = (data & 0x3f) |
 493                        (sensor_settings[VFLIP_IDX] << 7) |
 494                        ((val & 0x01) << 6);
 495         }
 496
 497         data = ((data & ~S5K4AA_RM_H_FLIP) | ((val & 0x01) << 6));
 498         err = m5602_write_sensor(sd, S5K4AA_READ_MODE, &data, 1);
 499         if (err < 0)
 500                 return err;
 501
 502         if (val) {
 503                 err = m5602_read_sensor(sd, S5K4AA_COLSTART_LO, &data, 1);
 504                 if (err < 0)
 505                         return err;
 506                 data++;
 507                 err = m5602_write_sensor(sd, S5K4AA_COLSTART_LO, &data, 1);
 508                 if (err < 0)
 509                         return err;
 510         } else {
 511                 err = m5602_read_sensor(sd, S5K4AA_COLSTART_LO, &data, 1);
 512                 if (err < 0)
 513                         return err;
 514                 data--;
 515                 err = m5602_write_sensor(sd, S5K4AA_COLSTART_LO, &data, 1);
 516         }
 517         return err;
 518 }
 519
 520 static int s5k4aa_get_gain(struct gspca_dev *gspca_dev, __s32 *val)
 521 {
 522         struct sd *sd = (struct sd *) gspca_dev;
 523         s32 *sensor_settings = sd->sensor_priv;
 524
 525         *val = sensor_settings[GAIN_IDX];
 526         PDEBUG(D_V4L2, "Read gain %d", *val);
 527         return 0;
 528 }
 529
 530 static int s5k4aa_set_gain(struct gspca_dev *gspca_dev, __s32 val)
 531 {
 532         struct sd *sd = (struct sd *) gspca_dev;
 533         s32 *sensor_settings = sd->sensor_priv;
 534         u8 data = S5K4AA_PAGE_MAP_2;
 535         int err;
 536
 537         sensor_settings[GAIN_IDX] = val;
 538
 539         PDEBUG(D_V4L2, "Set gain to %d", val);
 540         err = m5602_write_sensor(sd, S5K4AA_PAGE_MAP, &data, 1);
 541         if (err < 0)
 542                 return err;
 543
 544         data = val & 0xff;
 545         err = m5602_write_sensor(sd, S5K4AA_GAIN, &data, 1);
 546
 547         return err;
 548 }
 549
 550 static int s5k4aa_get_brightness(struct gspca_dev *gspca_dev, __s32 *val)
 551 {
 552         struct sd *sd = (struct sd *) gspca_dev;
 553         s32 *sensor_settings = sd->sensor_priv;
 554
 555         *val = sensor_settings[BRIGHTNESS_IDX];
 556         PDEBUG(D_V4L2, "Read brightness %d", *val);
 557         return 0;
 558 }
 559
 560 static int s5k4aa_set_brightness(struct gspca_dev *gspca_dev, __s32 val)
 561 {
 562         struct sd *sd = (struct sd *) gspca_dev;
 563         s32 *sensor_settings = sd->sensor_priv;
 564         u8 data = S5K4AA_PAGE_MAP_2;
 565         int err;
 566
 567         sensor_settings[BRIGHTNESS_IDX] = val;
 568
 569         PDEBUG(D_V4L2, "Set brightness to %d", val);
 570         err = m5602_write_sensor(sd, S5K4AA_PAGE_MAP, &data, 1);
 571         if (err < 0)
 572                 return err;
 573
 574         data = val & 0xff;
 575         return m5602_write_sensor(sd, S5K4AA_BRIGHTNESS, &data, 1);
 576 }
 577
 578 static int s5k4aa_get_noise(struct gspca_dev *gspca_dev, __s32 *val)
 579 {
 580         struct sd *sd = (struct sd *) gspca_dev;
 581         s32 *sensor_settings = sd->sensor_priv;
 582
 583         *val = sensor_settings[NOISE_SUPP_IDX];
 584         PDEBUG(D_V4L2, "Read noise %d", *val);
 585         return 0;
 586 }
 587
 588 static int s5k4aa_set_noise(struct gspca_dev *gspca_dev, __s32 val)
 589 {
 590         struct sd *sd = (struct sd *) gspca_dev;
 591         s32 *sensor_settings = sd->sensor_priv;
 592         u8 data = S5K4AA_PAGE_MAP_2;
 593         int err;
 594
 595         sensor_settings[NOISE_SUPP_IDX] = val;
 596
 597         PDEBUG(D_V4L2, "Set noise to %d", val);
 598         err = m5602_write_sensor(sd, S5K4AA_PAGE_MAP, &data, 1);
 599         if (err < 0)
 600                 return err;
 601
 602         data = val & 0x01;
 603         return m5602_write_sensor(sd, S5K4AA_NOISE_SUPP, &data, 1);
 604 }
 605
 606 void s5k4aa_disconnect(struct sd *sd)
 607 {
 608         sd->sensor = NULL;
 609         kfree(sd->sensor_priv);
 610 }
 611
 612 static void s5k4aa_dump_registers(struct sd *sd)
 613 {
 614         int address;
 615         u8 page, old_page;
 616         m5602_read_sensor(sd, S5K4AA_PAGE_MAP, &old_page, 1);
 617         for (page = 0; page < 16; page++) {
 618                 m5602_write_sensor(sd, S5K4AA_PAGE_MAP, &page, 1);
 619                 info("Dumping the s5k4aa register state for page 0x%x", page);
 620                 for (address = 0; address <= 0xff; address++) {
 621                         u8 value = 0;
 622                         m5602_read_sensor(sd, address, &value, 1);
 623                         info("register 0x%x contains 0x%x",
 624                              address, value);
 625                 }
 626         }
 627         info("s5k4aa register state dump complete");
 628
 629         for (page = 0; page < 16; page++) {
 630                 m5602_write_sensor(sd, S5K4AA_PAGE_MAP, &page, 1);
 631                 info("Probing for which registers that are "
 632                      "read/write for page 0x%x", page);
 633                 for (address = 0; address <= 0xff; address++) {
 634                         u8 old_value, ctrl_value, test_value = 0xff;
 635
 636                         m5602_read_sensor(sd, address, &old_value, 1);
 637                         m5602_write_sensor(sd, address, &test_value, 1);
 638                         m5602_read_sensor(sd, address, &ctrl_value, 1);
 639
 640                         if (ctrl_value == test_value)
 641                                 info("register 0x%x is writeable", address);
 642                         else
 643                                 info("register 0x%x is read only", address);
 644
 645                         /* Restore original value */
 646                         m5602_write_sensor(sd, address, &old_value, 1);
 647                 }
 648         }
 649         info("Read/write register probing complete");
 650         m5602_write_sensor(sd, S5K4AA_PAGE_MAP, &old_page, 1);
 651 }