drivers/media/i2c/vs6624.c

   1 /*
   2  * vs6624.c ST VS6624 CMOS image sensor driver
   3  *
   4  * Copyright (c) 2011 Analog Devices Inc.
   5  *
   6  * This program is free software; you can redistribute it and/or modify
   7  * it under the terms of the GNU General Public License version 2 as
   8  * published by the Free Software Foundation.
   9  *
  10  * This program is distributed in the hope that it will be useful,
  11  * but WITHOUT ANY WARRANTY; without even the implied warranty of
  12  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
  13  * GNU General Public License for more details.
  14  *
  15  * You should have received a copy of the GNU General Public License
  16  * along with this program; if not, write to the Free Software
  17  * Foundation, Inc., 675 Mass Ave, Cambridge, MA 02139, USA.
  18  */
  19
  20 #include <linux/delay.h>
  21 #include <linux/errno.h>
  22 #include <linux/gpio.h>
  23 #include <linux/i2c.h>
  24 #include <linux/init.h>
  25 #include <linux/module.h>
  26 #include <linux/slab.h>
  27 #include <linux/types.h>
  28 #include <linux/videodev2.h>
  29
  30 #include <media/v4l2-ctrls.h>
  31 #include <media/v4l2-device.h>
  32 #include <media/v4l2-mediabus.h>
  33
  34 #include "vs6624_regs.h"
  35
  36 #define VGA_WIDTH       640
  37 #define VGA_HEIGHT      480
  38 #define QVGA_WIDTH      320
  39 #define QVGA_HEIGHT     240
  40 #define QQVGA_WIDTH     160
  41 #define QQVGA_HEIGHT    120
  42 #define CIF_WIDTH       352
  43 #define CIF_HEIGHT      288
  44 #define QCIF_WIDTH      176
  45 #define QCIF_HEIGHT     144
  46 #define QQCIF_WIDTH     88
  47 #define QQCIF_HEIGHT    72
  48
  49 #define MAX_FRAME_RATE  30
  50
  51 struct vs6624 {
  52         struct v4l2_subdev sd;
  53         struct v4l2_ctrl_handler hdl;
  54         struct v4l2_fract frame_rate;
  55         struct v4l2_mbus_framefmt fmt;
  56         unsigned ce_pin;
  57 };
  58
  59 static const struct vs6624_format {
  60         enum v4l2_mbus_pixelcode mbus_code;
  61         enum v4l2_colorspace colorspace;
  62 } vs6624_formats[] = {
  63         {
  64                 .mbus_code      = V4L2_MBUS_FMT_UYVY8_2X8,
  65                 .colorspace     = V4L2_COLORSPACE_JPEG,
  66         },
  67         {
  68                 .mbus_code      = V4L2_MBUS_FMT_YUYV8_2X8,
  69                 .colorspace     = V4L2_COLORSPACE_JPEG,
  70         },
  71         {
  72                 .mbus_code      = V4L2_MBUS_FMT_RGB565_2X8_LE,
  73                 .colorspace     = V4L2_COLORSPACE_SRGB,
  74         },
  75 };
  76
  77 static struct v4l2_mbus_framefmt vs6624_default_fmt = {
  78         .width = VGA_WIDTH,
  79         .height = VGA_HEIGHT,
  80         .code = V4L2_MBUS_FMT_UYVY8_2X8,
  81         .field = V4L2_FIELD_NONE,
  82         .colorspace = V4L2_COLORSPACE_JPEG,
  83 };
  84
  85 static const u16 vs6624_p1[] = {
  86         0x8104, 0x03,
  87         0x8105, 0x01,
  88         0xc900, 0x03,
  89         0xc904, 0x47,
  90         0xc905, 0x10,
  91         0xc906, 0x80,
  92         0xc907, 0x3a,
  93         0x903a, 0x02,
  94         0x903b, 0x47,
  95         0x903c, 0x15,
  96         0xc908, 0x31,
  97         0xc909, 0xdc,
  98         0xc90a, 0x80,
  99         0xc90b, 0x44,
 100         0x9044, 0x02,
 101         0x9045, 0x31,
 102         0x9046, 0xe2,
 103         0xc90c, 0x07,
 104         0xc90d, 0xe0,
 105         0xc90e, 0x80,
 106         0xc90f, 0x47,
 107         0x9047, 0x90,
 108         0x9048, 0x83,
 109         0x9049, 0x81,
 110         0x904a, 0xe0,
 111         0x904b, 0x60,
 112         0x904c, 0x08,
 113         0x904d, 0x90,
 114         0x904e, 0xc0,
 115         0x904f, 0x43,
 116         0x9050, 0x74,
 117         0x9051, 0x01,
 118         0x9052, 0xf0,
 119         0x9053, 0x80,
 120         0x9054, 0x05,
 121         0x9055, 0xE4,
 122         0x9056, 0x90,
 123         0x9057, 0xc0,
 124         0x9058, 0x43,
 125         0x9059, 0xf0,
 126         0x905a, 0x02,
 127         0x905b, 0x07,
 128         0x905c, 0xec,
 129         0xc910, 0x5d,
 130         0xc911, 0xca,
 131         0xc912, 0x80,
 132         0xc913, 0x5d,
 133         0x905d, 0xa3,
 134         0x905e, 0x04,
 135         0x905f, 0xf0,
 136         0x9060, 0xa3,
 137         0x9061, 0x04,
 138         0x9062, 0xf0,
 139         0x9063, 0x22,
 140         0xc914, 0x72,
 141         0xc915, 0x92,
 142         0xc916, 0x80,
 143         0xc917, 0x64,
 144         0x9064, 0x74,
 145         0x9065, 0x01,
 146         0x9066, 0x02,
 147         0x9067, 0x72,
 148         0x9068, 0x95,
 149         0xc918, 0x47,
 150         0xc919, 0xf2,
 151         0xc91a, 0x81,
 152         0xc91b, 0x69,
 153         0x9169, 0x74,
 154         0x916a, 0x02,
 155         0x916b, 0xf0,
 156         0x916c, 0xec,
 157         0x916d, 0xb4,
 158         0x916e, 0x10,
 159         0x916f, 0x0a,
 160         0x9170, 0x90,
 161         0x9171, 0x80,
 162         0x9172, 0x16,
 163         0x9173, 0xe0,
 164         0x9174, 0x70,
 165         0x9175, 0x04,
 166         0x9176, 0x90,
 167         0x9177, 0xd3,
 168         0x9178, 0xc4,
 169         0x9179, 0xf0,
 170         0x917a, 0x22,
 171         0xc91c, 0x0a,
 172         0xc91d, 0xbe,
 173         0xc91e, 0x80,
 174         0xc91f, 0x73,
 175         0x9073, 0xfc,
 176         0x9074, 0xa3,
 177         0x9075, 0xe0,
 178         0x9076, 0xf5,
 179         0x9077, 0x82,
 180         0x9078, 0x8c,
 181         0x9079, 0x83,
 182         0x907a, 0xa3,
 183         0x907b, 0xa3,
 184         0x907c, 0xe0,
 185         0x907d, 0xfc,
 186         0x907e, 0xa3,
 187         0x907f, 0xe0,
 188         0x9080, 0xc3,
 189         0x9081, 0x9f,
 190         0x9082, 0xff,
 191         0x9083, 0xec,
 192         0x9084, 0x9e,
 193         0x9085, 0xfe,
 194         0x9086, 0x02,
 195         0x9087, 0x0a,
 196         0x9088, 0xea,
 197         0xc920, 0x47,
 198         0xc921, 0x38,
 199         0xc922, 0x80,
 200         0xc923, 0x89,
 201         0x9089, 0xec,
 202         0x908a, 0xd3,
 203         0x908b, 0x94,
 204         0x908c, 0x20,
 205         0x908d, 0x40,
 206         0x908e, 0x01,
 207         0x908f, 0x1c,
 208         0x9090, 0x90,
 209         0x9091, 0xd3,
 210         0x9092, 0xd4,
 211         0x9093, 0xec,
 212         0x9094, 0xf0,
 213         0x9095, 0x02,
 214         0x9096, 0x47,
 215         0x9097, 0x3d,
 216         0xc924, 0x45,
 217         0xc925, 0xca,
 218         0xc926, 0x80,
 219         0xc927, 0x98,
 220         0x9098, 0x12,
 221         0x9099, 0x77,
 222         0x909a, 0xd6,
 223         0x909b, 0x02,
 224         0x909c, 0x45,
 225         0x909d, 0xcd,
 226         0xc928, 0x20,
 227         0xc929, 0xd5,
 228         0xc92a, 0x80,
 229         0xc92b, 0x9e,
 230         0x909e, 0x90,
 231         0x909f, 0x82,
 232         0x90a0, 0x18,
 233         0x90a1, 0xe0,
 234         0x90a2, 0xb4,
 235         0x90a3, 0x03,
 236         0x90a4, 0x0e,
 237         0x90a5, 0x90,
 238         0x90a6, 0x83,
 239         0x90a7, 0xbf,
 240         0x90a8, 0xe0,
 241         0x90a9, 0x60,
 242         0x90aa, 0x08,
 243         0x90ab, 0x90,
 244         0x90ac, 0x81,
 245         0x90ad, 0xfc,
 246         0x90ae, 0xe0,
 247         0x90af, 0xff,
 248         0x90b0, 0xc3,
 249         0x90b1, 0x13,
 250         0x90b2, 0xf0,
 251         0x90b3, 0x90,
 252         0x90b4, 0x81,
 253         0x90b5, 0xfc,
 254         0x90b6, 0xe0,
 255         0x90b7, 0xff,
 256         0x90b8, 0x02,
 257         0x90b9, 0x20,
 258         0x90ba, 0xda,
 259         0xc92c, 0x70,
 260         0xc92d, 0xbc,
 261         0xc92e, 0x80,
 262         0xc92f, 0xbb,
 263         0x90bb, 0x90,
 264         0x90bc, 0x82,
 265         0x90bd, 0x18,
 266         0x90be, 0xe0,
 267         0x90bf, 0xb4,
 268         0x90c0, 0x03,
 269         0x90c1, 0x06,
 270         0x90c2, 0x90,
 271         0x90c3, 0xc1,
 272         0x90c4, 0x06,
 273         0x90c5, 0x74,
 274         0x90c6, 0x05,
 275         0x90c7, 0xf0,
 276         0x90c8, 0x90,
 277         0x90c9, 0xd3,
 278         0x90ca, 0xa0,
 279         0x90cb, 0x02,
 280         0x90cc, 0x70,
 281         0x90cd, 0xbf,
 282         0xc930, 0x72,
 283         0xc931, 0x21,
 284         0xc932, 0x81,
 285         0xc933, 0x3b,
 286         0x913b, 0x7d,
 287         0x913c, 0x02,
 288         0x913d, 0x7f,
 289         0x913e, 0x7b,
 290         0x913f, 0x02,
 291         0x9140, 0x72,
 292         0x9141, 0x25,
 293         0xc934, 0x28,
 294         0xc935, 0xae,
 295         0xc936, 0x80,
 296         0xc937, 0xd2,
 297         0x90d2, 0xf0,
 298         0x90d3, 0x90,
 299         0x90d4, 0xd2,
 300         0x90d5, 0x0a,
 301         0x90d6, 0x02,
 302         0x90d7, 0x28,
 303         0x90d8, 0xb4,
 304         0xc938, 0x28,
 305         0xc939, 0xb1,
 306         0xc93a, 0x80,
 307         0xc93b, 0xd9,
 308         0x90d9, 0x90,
 309         0x90da, 0x83,
 310         0x90db, 0xba,
 311         0x90dc, 0xe0,
 312         0x90dd, 0xff,
 313         0x90de, 0x90,
 314         0x90df, 0xd2,
 315         0x90e0, 0x08,
 316         0x90e1, 0xe0,
 317         0x90e2, 0xe4,
 318         0x90e3, 0xef,
 319         0x90e4, 0xf0,
 320         0x90e5, 0xa3,
 321         0x90e6, 0xe0,
 322         0x90e7, 0x74,
 323         0x90e8, 0xff,
 324         0x90e9, 0xf0,
 325         0x90ea, 0x90,
 326         0x90eb, 0xd2,
 327         0x90ec, 0x0a,
 328         0x90ed, 0x02,
 329         0x90ee, 0x28,
 330         0x90ef, 0xb4,
 331         0xc93c, 0x29,
 332         0xc93d, 0x79,
 333         0xc93e, 0x80,
 334         0xc93f, 0xf0,
 335         0x90f0, 0xf0,
 336         0x90f1, 0x90,
 337         0x90f2, 0xd2,
 338         0x90f3, 0x0e,
 339         0x90f4, 0x02,
 340         0x90f5, 0x29,
 341         0x90f6, 0x7f,
 342         0xc940, 0x29,
 343         0xc941, 0x7c,
 344         0xc942, 0x80,
 345         0xc943, 0xf7,
 346         0x90f7, 0x90,
 347         0x90f8, 0x83,
 348         0x90f9, 0xba,
 349         0x90fa, 0xe0,
 350         0x90fb, 0xff,
 351         0x90fc, 0x90,
 352         0x90fd, 0xd2,
 353         0x90fe, 0x0c,
 354         0x90ff, 0xe0,
 355         0x9100, 0xe4,
 356         0x9101, 0xef,
 357         0x9102, 0xf0,
 358         0x9103, 0xa3,
 359         0x9104, 0xe0,
 360         0x9105, 0x74,
 361         0x9106, 0xff,
 362         0x9107, 0xf0,
 363         0x9108, 0x90,
 364         0x9109, 0xd2,
 365         0x910a, 0x0e,
 366         0x910b, 0x02,
 367         0x910c, 0x29,
 368         0x910d, 0x7f,
 369         0xc944, 0x2a,
 370         0xc945, 0x42,
 371         0xc946, 0x81,
 372         0xc947, 0x0e,
 373         0x910e, 0xf0,
 374         0x910f, 0x90,
 375         0x9110, 0xd2,
 376         0x9111, 0x12,
 377         0x9112, 0x02,
 378         0x9113, 0x2a,
 379         0x9114, 0x48,
 380         0xc948, 0x2a,
 381         0xc949, 0x45,
 382         0xc94a, 0x81,
 383         0xc94b, 0x15,
 384         0x9115, 0x90,
 385         0x9116, 0x83,
 386         0x9117, 0xba,
 387         0x9118, 0xe0,
 388         0x9119, 0xff,
 389         0x911a, 0x90,
 390         0x911b, 0xd2,
 391         0x911c, 0x10,
 392         0x911d, 0xe0,
 393         0x911e, 0xe4,
 394         0x911f, 0xef,
 395         0x9120, 0xf0,
 396         0x9121, 0xa3,
 397         0x9122, 0xe0,
 398         0x9123, 0x74,
 399         0x9124, 0xff,
 400         0x9125, 0xf0,
 401         0x9126, 0x90,
 402         0x9127, 0xd2,
 403         0x9128, 0x12,
 404         0x9129, 0x02,
 405         0x912a, 0x2a,
 406         0x912b, 0x48,
 407         0xc900, 0x01,
 408         0x0000, 0x00,
 409 };
 410
 411 static const u16 vs6624_p2[] = {
 412         0x806f, 0x01,
 413         0x058c, 0x01,
 414         0x0000, 0x00,
 415 };
 416
 417 static const u16 vs6624_run_setup[] = {
 418         0x1d18, 0x00,                           /* Enableconstrainedwhitebalance */
 419         VS6624_PEAK_MIN_OUT_G_MSB, 0x3c,        /* Damper PeakGain Output MSB */
 420         VS6624_PEAK_MIN_OUT_G_LSB, 0x66,        /* Damper PeakGain Output LSB */
 421         VS6624_CM_LOW_THR_MSB, 0x65,            /* Damper Low MSB */
 422         VS6624_CM_LOW_THR_LSB, 0xd1,            /* Damper Low LSB */
 423         VS6624_CM_HIGH_THR_MSB, 0x66,           /* Damper High MSB */
 424         VS6624_CM_HIGH_THR_LSB, 0x62,           /* Damper High LSB */
 425         VS6624_CM_MIN_OUT_MSB, 0x00,            /* Damper Min output MSB */
 426         VS6624_CM_MIN_OUT_LSB, 0x00,            /* Damper Min output LSB */
 427         VS6624_NORA_DISABLE, 0x00,              /* Nora fDisable */
 428         VS6624_NORA_USAGE, 0x04,                /* Nora usage */
 429         VS6624_NORA_LOW_THR_MSB, 0x63,          /* Damper Low MSB Changed 0x63 to 0x65 */
 430         VS6624_NORA_LOW_THR_LSB, 0xd1,          /* Damper Low LSB */
 431         VS6624_NORA_HIGH_THR_MSB, 0x68,         /* Damper High MSB */
 432         VS6624_NORA_HIGH_THR_LSB, 0xdd,         /* Damper High LSB */
 433         VS6624_NORA_MIN_OUT_MSB, 0x3a,          /* Damper Min output MSB */
 434         VS6624_NORA_MIN_OUT_LSB, 0x00,          /* Damper Min output LSB */
 435         VS6624_F2B_DISABLE, 0x00,               /* Disable */
 436         0x1d8a, 0x30,                           /* MAXWeightHigh */
 437         0x1d91, 0x62,                           /* fpDamperLowThresholdHigh MSB */
 438         0x1d92, 0x4a,                           /* fpDamperLowThresholdHigh LSB */
 439         0x1d95, 0x65,                           /* fpDamperHighThresholdHigh MSB */
 440         0x1d96, 0x0e,                           /* fpDamperHighThresholdHigh LSB */
 441         0x1da1, 0x3a,                           /* fpMinimumDamperOutputLow MSB */
 442         0x1da2, 0xb8,                           /* fpMinimumDamperOutputLow LSB */
 443         0x1e08, 0x06,                           /* MAXWeightLow */
 444         0x1e0a, 0x0a,                           /* MAXWeightHigh */
 445         0x1601, 0x3a,                           /* Red A MSB */
 446         0x1602, 0x14,                           /* Red A LSB */
 447         0x1605, 0x3b,                           /* Blue A MSB */
 448         0x1606, 0x85,                           /* BLue A LSB */
 449         0x1609, 0x3b,                           /* RED B MSB */
 450         0x160a, 0x85,                           /* RED B LSB */
 451         0x160d, 0x3a,                           /* Blue B MSB */
 452         0x160e, 0x14,                           /* Blue B LSB */
 453         0x1611, 0x30,                           /* Max Distance from Locus MSB */
 454         0x1612, 0x8f,                           /* Max Distance from Locus MSB */
 455         0x1614, 0x01,                           /* Enable constrainer */
 456         0x0000, 0x00,
 457 };
 458
 459 static const u16 vs6624_default[] = {
 460         VS6624_CONTRAST0, 0x84,
 461         VS6624_SATURATION0, 0x75,
 462         VS6624_GAMMA0, 0x11,
 463         VS6624_CONTRAST1, 0x84,
 464         VS6624_SATURATION1, 0x75,
 465         VS6624_GAMMA1, 0x11,
 466         VS6624_MAN_RG, 0x80,
 467         VS6624_MAN_GG, 0x80,
 468         VS6624_MAN_BG, 0x80,
 469         VS6624_WB_MODE, 0x1,
 470         VS6624_EXPO_COMPENSATION, 0xfe,
 471         VS6624_EXPO_METER, 0x0,
 472         VS6624_LIGHT_FREQ, 0x64,
 473         VS6624_PEAK_GAIN, 0xe,
 474         VS6624_PEAK_LOW_THR, 0x28,
 475         VS6624_HMIRROR0, 0x0,
 476         VS6624_VFLIP0, 0x0,
 477         VS6624_ZOOM_HSTEP0_MSB, 0x0,
 478         VS6624_ZOOM_HSTEP0_LSB, 0x1,
 479         VS6624_ZOOM_VSTEP0_MSB, 0x0,
 480         VS6624_ZOOM_VSTEP0_LSB, 0x1,
 481         VS6624_PAN_HSTEP0_MSB, 0x0,
 482         VS6624_PAN_HSTEP0_LSB, 0xf,
 483         VS6624_PAN_VSTEP0_MSB, 0x0,
 484         VS6624_PAN_VSTEP0_LSB, 0xf,
 485         VS6624_SENSOR_MODE, 0x1,
 486         VS6624_SYNC_CODE_SETUP, 0x21,
 487         VS6624_DISABLE_FR_DAMPER, 0x0,
 488         VS6624_FR_DEN, 0x1,
 489         VS6624_FR_NUM_LSB, 0xf,
 490         VS6624_INIT_PIPE_SETUP, 0x0,
 491         VS6624_IMG_FMT0, 0x0,
 492         VS6624_YUV_SETUP, 0x1,
 493         VS6624_IMAGE_SIZE0, 0x2,
 494         0x0000, 0x00,
 495 };
 496
 497 static inline struct vs6624 *to_vs6624(struct v4l2_subdev *sd)
 498 {
 499         return container_of(sd, struct vs6624, sd);
 500 }
 501 static inline struct v4l2_subdev *to_sd(struct v4l2_ctrl *ctrl)
 502 {
 503         return &container_of(ctrl->handler, struct vs6624, hdl)->sd;
 504 }
 505
 506 static int vs6624_read(struct v4l2_subdev *sd, u16 index)
 507 {
 508         struct i2c_client *client = v4l2_get_subdevdata(sd);
 509         u8 buf[2];
 510
 511         buf[0] = index >> 8;
 512         buf[1] = index;
 513         i2c_master_send(client, buf, 2);
 514         i2c_master_recv(client, buf, 1);
 515
 516         return buf[0];
 517 }
 518
 519 static int vs6624_write(struct v4l2_subdev *sd, u16 index,
 520                                 u8 value)
 521 {
 522         struct i2c_client *client = v4l2_get_subdevdata(sd);
 523         u8 buf[3];
 524
 525         buf[0] = index >> 8;
 526         buf[1] = index;
 527         buf[2] = value;
 528
 529         return i2c_master_send(client, buf, 3);
 530 }
 531
 532 static int vs6624_writeregs(struct v4l2_subdev *sd, const u16 *regs)
 533 {
 534         u16 reg;
 535         u8 data;
 536
 537         while (*regs != 0x00) {
 538                 reg = *regs++;
 539                 data = *regs++;
 540
 541                 vs6624_write(sd, reg, data);
 542         }
 543         return 0;
 544 }
 545
 546 static int vs6624_s_ctrl(struct v4l2_ctrl *ctrl)
 547 {
 548         struct v4l2_subdev *sd = to_sd(ctrl);
 549
 550         switch (ctrl->id) {
 551         case V4L2_CID_CONTRAST:
 552                 vs6624_write(sd, VS6624_CONTRAST0, ctrl->val);
 553                 break;
 554         case V4L2_CID_SATURATION:
 555                 vs6624_write(sd, VS6624_SATURATION0, ctrl->val);
 556                 break;
 557         case V4L2_CID_HFLIP:
 558                 vs6624_write(sd, VS6624_HMIRROR0, ctrl->val);
 559                 break;
 560         case V4L2_CID_VFLIP:
 561                 vs6624_write(sd, VS6624_VFLIP0, ctrl->val);
 562                 break;
 563         default:
 564                 return -EINVAL;
 565         }
 566
 567         return 0;
 568 }
 569
 570 static int vs6624_enum_mbus_fmt(struct v4l2_subdev *sd, unsigned index,
 571                                 enum v4l2_mbus_pixelcode *code)
 572 {
 573         if (index >= ARRAY_SIZE(vs6624_formats))
 574                 return -EINVAL;
 575
 576         *code = vs6624_formats[index].mbus_code;
 577         return 0;
 578 }
 579
 580 static int vs6624_try_mbus_fmt(struct v4l2_subdev *sd,
 581                                 struct v4l2_mbus_framefmt *fmt)
 582 {
 583         int index;
 584
 585         for (index = 0; index < ARRAY_SIZE(vs6624_formats); index++)
 586                 if (vs6624_formats[index].mbus_code == fmt->code)
 587                         break;
 588         if (index >= ARRAY_SIZE(vs6624_formats)) {
 589                 /* default to first format */
 590                 index = 0;
 591                 fmt->code = vs6624_formats[0].mbus_code;
 592         }
 593
 594         /* sensor mode is VGA */
 595         if (fmt->width > VGA_WIDTH)
 596                 fmt->width = VGA_WIDTH;
 597         if (fmt->height > VGA_HEIGHT)
 598                 fmt->height = VGA_HEIGHT;
 599         fmt->width = fmt->width & (~3);
 600         fmt->height = fmt->height & (~3);
 601         fmt->field = V4L2_FIELD_NONE;
 602         fmt->colorspace = vs6624_formats[index].colorspace;
 603         return 0;
 604 }
 605
 606 static int vs6624_s_mbus_fmt(struct v4l2_subdev *sd,
 607                                 struct v4l2_mbus_framefmt *fmt)
 608 {
 609         struct vs6624 *sensor = to_vs6624(sd);
 610         int ret;
 611
 612         ret = vs6624_try_mbus_fmt(sd, fmt);
 613         if (ret)
 614                 return ret;
 615
 616         /* set image format */
 617         switch (fmt->code) {
 618         case V4L2_MBUS_FMT_UYVY8_2X8:
 619                 vs6624_write(sd, VS6624_IMG_FMT0, 0x0);
 620                 vs6624_write(sd, VS6624_YUV_SETUP, 0x1);
 621                 break;
 622         case V4L2_MBUS_FMT_YUYV8_2X8:
 623                 vs6624_write(sd, VS6624_IMG_FMT0, 0x0);
 624                 vs6624_write(sd, VS6624_YUV_SETUP, 0x3);
 625                 break;
 626         case V4L2_MBUS_FMT_RGB565_2X8_LE:
 627                 vs6624_write(sd, VS6624_IMG_FMT0, 0x4);
 628                 vs6624_write(sd, VS6624_RGB_SETUP, 0x0);
 629                 break;
 630         default:
 631                 return -EINVAL;
 632         }
 633
 634         /* set image size */
 635         if ((fmt->width == VGA_WIDTH) && (fmt->height == VGA_HEIGHT))
 636                 vs6624_write(sd, VS6624_IMAGE_SIZE0, 0x2);
 637         else if ((fmt->width == QVGA_WIDTH) && (fmt->height == QVGA_HEIGHT))
 638                 vs6624_write(sd, VS6624_IMAGE_SIZE0, 0x4);
 639         else if ((fmt->width == QQVGA_WIDTH) && (fmt->height == QQVGA_HEIGHT))
 640                 vs6624_write(sd, VS6624_IMAGE_SIZE0, 0x6);
 641         else if ((fmt->width == CIF_WIDTH) && (fmt->height == CIF_HEIGHT))
 642                 vs6624_write(sd, VS6624_IMAGE_SIZE0, 0x3);
 643         else if ((fmt->width == QCIF_WIDTH) && (fmt->height == QCIF_HEIGHT))
 644                 vs6624_write(sd, VS6624_IMAGE_SIZE0, 0x5);
 645         else if ((fmt->width == QQCIF_WIDTH) && (fmt->height == QQCIF_HEIGHT))
 646                 vs6624_write(sd, VS6624_IMAGE_SIZE0, 0x7);
 647         else {
 648                 vs6624_write(sd, VS6624_IMAGE_SIZE0, 0x8);
 649                 vs6624_write(sd, VS6624_MAN_HSIZE0_MSB, fmt->width >> 8);
 650                 vs6624_write(sd, VS6624_MAN_HSIZE0_LSB, fmt->width & 0xFF);
 651                 vs6624_write(sd, VS6624_MAN_VSIZE0_MSB, fmt->height >> 8);
 652                 vs6624_write(sd, VS6624_MAN_VSIZE0_LSB, fmt->height & 0xFF);
 653                 vs6624_write(sd, VS6624_CROP_CTRL0, 0x1);
 654         }
 655
 656         sensor->fmt = *fmt;
 657
 658         return 0;
 659 }
 660
 661 static int vs6624_g_mbus_fmt(struct v4l2_subdev *sd,
 662                                 struct v4l2_mbus_framefmt *fmt)
 663 {
 664         struct vs6624 *sensor = to_vs6624(sd);
 665
 666         *fmt = sensor->fmt;
 667         return 0;
 668 }
 669
 670 static int vs6624_g_parm(struct v4l2_subdev *sd, struct v4l2_streamparm *parms)
 671 {
 672         struct vs6624 *sensor = to_vs6624(sd);
 673         struct v4l2_captureparm *cp = &parms->parm.capture;
 674
 675         if (parms->type != V4L2_BUF_TYPE_VIDEO_CAPTURE)
 676                 return -EINVAL;
 677
 678         memset(cp, 0, sizeof(*cp));
 679         cp->capability = V4L2_CAP_TIMEPERFRAME;
 680         cp->timeperframe.numerator = sensor->frame_rate.denominator;
 681         cp->timeperframe.denominator = sensor->frame_rate.numerator;
 682         return 0;
 683 }
 684
 685 static int vs6624_s_parm(struct v4l2_subdev *sd, struct v4l2_streamparm *parms)
 686 {
 687         struct vs6624 *sensor = to_vs6624(sd);
 688         struct v4l2_captureparm *cp = &parms->parm.capture;
 689         struct v4l2_fract *tpf = &cp->timeperframe;
 690
 691         if (parms->type != V4L2_BUF_TYPE_VIDEO_CAPTURE)
 692                 return -EINVAL;
 693         if (cp->extendedmode != 0)
 694                 return -EINVAL;
 695
 696         if (tpf->numerator == 0 || tpf->denominator == 0
 697                 || (tpf->denominator > tpf->numerator * MAX_FRAME_RATE)) {
 698                 /* reset to max frame rate */
 699                 tpf->numerator = 1;
 700                 tpf->denominator = MAX_FRAME_RATE;
 701         }
 702         sensor->frame_rate.numerator = tpf->denominator;
 703         sensor->frame_rate.denominator = tpf->numerator;
 704         vs6624_write(sd, VS6624_DISABLE_FR_DAMPER, 0x0);
 705         vs6624_write(sd, VS6624_FR_NUM_MSB,
 706                         sensor->frame_rate.numerator >> 8);
 707         vs6624_write(sd, VS6624_FR_NUM_LSB,
 708                         sensor->frame_rate.numerator & 0xFF);
 709         vs6624_write(sd, VS6624_FR_DEN,
 710                         sensor->frame_rate.denominator & 0xFF);
 711         return 0;
 712 }
 713
 714 static int vs6624_s_stream(struct v4l2_subdev *sd, int enable)
 715 {
 716         if (enable)
 717                 vs6624_write(sd, VS6624_USER_CMD, 0x2);
 718         else
 719                 vs6624_write(sd, VS6624_USER_CMD, 0x4);
 720         udelay(100);
 721         return 0;
 722 }
 723
 724 #ifdef CONFIG_VIDEO_ADV_DEBUG
 725 static int vs6624_g_register(struct v4l2_subdev *sd, struct v4l2_dbg_register *reg)
 726 {
 727         reg->val = vs6624_read(sd, reg->reg & 0xffff);
 728         reg->size = 1;
 729         return 0;
 730 }
 731
 732 static int vs6624_s_register(struct v4l2_subdev *sd, const struct v4l2_dbg_register *reg)
 733 {
 734         vs6624_write(sd, reg->reg & 0xffff, reg->val & 0xff);
 735         return 0;
 736 }
 737 #endif
 738
 739 static const struct v4l2_ctrl_ops vs6624_ctrl_ops = {
 740         .s_ctrl = vs6624_s_ctrl,
 741 };
 742
 743 static const struct v4l2_subdev_core_ops vs6624_core_ops = {
 744 #ifdef CONFIG_VIDEO_ADV_DEBUG
 745         .g_register = vs6624_g_register,
 746         .s_register = vs6624_s_register,
 747 #endif
 748 };
 749
 750 static const struct v4l2_subdev_video_ops vs6624_video_ops = {
 751         .enum_mbus_fmt = vs6624_enum_mbus_fmt,
 752         .try_mbus_fmt = vs6624_try_mbus_fmt,
 753         .s_mbus_fmt = vs6624_s_mbus_fmt,
 754         .g_mbus_fmt = vs6624_g_mbus_fmt,
 755         .s_parm = vs6624_s_parm,
 756         .g_parm = vs6624_g_parm,
 757         .s_stream = vs6624_s_stream,
 758 };
 759
 760 static const struct v4l2_subdev_ops vs6624_ops = {
 761         .core = &vs6624_core_ops,
 762         .video = &vs6624_video_ops,
 763 };
 764
 765 static int vs6624_probe(struct i2c_client *client,
 766                         const struct i2c_device_id *id)
 767 {
 768         struct vs6624 *sensor;
 769         struct v4l2_subdev *sd;
 770         struct v4l2_ctrl_handler *hdl;
 771         const unsigned *ce;
 772         int ret;
 773
 774         /* Check if the adapter supports the needed features */
 775         if (!i2c_check_functionality(client->adapter, I2C_FUNC_I2C))
 776                 return -EIO;
 777
 778         ce = client->dev.platform_data;
 779         if (ce == NULL)
 780                 return -EINVAL;
 781
 782         ret = devm_gpio_request_one(&client->dev, *ce, GPIOF_OUT_INIT_HIGH,
 783                                     "VS6624 Chip Enable");
 784         if (ret) {
 785                 v4l_err(client, "failed to request GPIO %d\n", *ce);
 786                 return ret;
 787         }
 788         /* wait 100ms before any further i2c writes are performed */
 789         mdelay(100);
 790
 791         sensor = devm_kzalloc(&client->dev, sizeof(*sensor), GFP_KERNEL);
 792         if (sensor == NULL)
 793                 return -ENOMEM;
 794
 795         sd = &sensor->sd;
 796         v4l2_i2c_subdev_init(sd, client, &vs6624_ops);
 797
 798         vs6624_writeregs(sd, vs6624_p1);
 799         vs6624_write(sd, VS6624_MICRO_EN, 0x2);
 800         vs6624_write(sd, VS6624_DIO_EN, 0x1);
 801         mdelay(10);
 802         vs6624_writeregs(sd, vs6624_p2);
 803
 804         vs6624_writeregs(sd, vs6624_default);
 805         vs6624_write(sd, VS6624_HSYNC_SETUP, 0xF);
 806         vs6624_writeregs(sd, vs6624_run_setup);
 807
 808         /* set frame rate */
 809         sensor->frame_rate.numerator = MAX_FRAME_RATE;
 810         sensor->frame_rate.denominator = 1;
 811         vs6624_write(sd, VS6624_DISABLE_FR_DAMPER, 0x0);
 812         vs6624_write(sd, VS6624_FR_NUM_MSB,
 813                         sensor->frame_rate.numerator >> 8);
 814         vs6624_write(sd, VS6624_FR_NUM_LSB,
 815                         sensor->frame_rate.numerator & 0xFF);
 816         vs6624_write(sd, VS6624_FR_DEN,
 817                         sensor->frame_rate.denominator & 0xFF);
 818
 819         sensor->fmt = vs6624_default_fmt;
 820         sensor->ce_pin = *ce;
 821
 822         v4l_info(client, "chip found @ 0x%02x (%s)\n",
 823                         client->addr << 1, client->adapter->name);
 824
 825         hdl = &sensor->hdl;
 826         v4l2_ctrl_handler_init(hdl, 4);
 827         v4l2_ctrl_new_std(hdl, &vs6624_ctrl_ops,
 828                         V4L2_CID_CONTRAST, 0, 0xFF, 1, 0x87);
 829         v4l2_ctrl_new_std(hdl, &vs6624_ctrl_ops,
 830                         V4L2_CID_SATURATION, 0, 0xFF, 1, 0x78);
 831         v4l2_ctrl_new_std(hdl, &vs6624_ctrl_ops,
 832                         V4L2_CID_HFLIP, 0, 1, 1, 0);
 833         v4l2_ctrl_new_std(hdl, &vs6624_ctrl_ops,
 834                         V4L2_CID_VFLIP, 0, 1, 1, 0);
 835         /* hook the control handler into the driver */
 836         sd->ctrl_handler = hdl;
 837         if (hdl->error) {
 838                 int err = hdl->error;
 839
 840                 v4l2_ctrl_handler_free(hdl);
 841                 return err;
 842         }
 843
 844         /* initialize the hardware to the default control values */
 845         ret = v4l2_ctrl_handler_setup(hdl);
 846         if (ret)
 847                 v4l2_ctrl_handler_free(hdl);
 848         return ret;
 849 }
 850
 851 static int vs6624_remove(struct i2c_client *client)
 852 {
 853         struct v4l2_subdev *sd = i2c_get_clientdata(client);
 854
 855         v4l2_device_unregister_subdev(sd);
 856         v4l2_ctrl_handler_free(sd->ctrl_handler);
 857         return 0;
 858 }
 859
 860 static const struct i2c_device_id vs6624_id[] = {
 861         {"vs6624", 0},
 862         {},
 863 };
 864
 865 MODULE_DEVICE_TABLE(i2c, vs6624_id);
 866
 867 static struct i2c_driver vs6624_driver = {
 868         .driver = {
 869                 .owner  = THIS_MODULE,
 870                 .name   = "vs6624",
 871         },
 872         .probe          = vs6624_probe,
 873         .remove         = vs6624_remove,
 874         .id_table       = vs6624_id,
 875 };
 876
 877 module_i2c_driver(vs6624_driver);
 878
 879 MODULE_DESCRIPTION("VS6624 sensor driver");
 880 MODULE_AUTHOR("Scott Jiang <Scott.Jiang.Linux@gmail.com>");
 881 MODULE_LICENSE("GPL v2");