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