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