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_code(struct v4l2_subdev *sd,
 561                 struct v4l2_subdev_pad_config *cfg,
 562                 struct v4l2_subdev_mbus_code_enum *code)
 563 {
 564         if (code->pad || code->index >= ARRAY_SIZE(vs6624_formats))
 565                 return -EINVAL;
 566
 567         code->code = vs6624_formats[code->index].mbus_code;
 568         return 0;
 569 }
 570
 571 static int vs6624_set_fmt(struct v4l2_subdev *sd,
 572                 struct v4l2_subdev_pad_config *cfg,
 573                 struct v4l2_subdev_format *format)
 574 {
 575         struct v4l2_mbus_framefmt *fmt = &format->format;
 576         struct vs6624 *sensor = to_vs6624(sd);
 577         int index;
 578
 579         if (format->pad)
 580                 return -EINVAL;
 581
 582         for (index = 0; index < ARRAY_SIZE(vs6624_formats); index++)
 583                 if (vs6624_formats[index].mbus_code == fmt->code)
 584                         break;
 585         if (index >= ARRAY_SIZE(vs6624_formats)) {
 586                 /* default to first format */
 587                 index = 0;
 588                 fmt->code = vs6624_formats[0].mbus_code;
 589         }
 590
 591         /* sensor mode is VGA */
 592         if (fmt->width > VGA_WIDTH)
 593                 fmt->width = VGA_WIDTH;
 594         if (fmt->height > VGA_HEIGHT)
 595                 fmt->height = VGA_HEIGHT;
 596         fmt->width = fmt->width & (~3);
 597         fmt->height = fmt->height & (~3);
 598         fmt->field = V4L2_FIELD_NONE;
 599         fmt->colorspace = vs6624_formats[index].colorspace;
 600
 601         if (format->which == V4L2_SUBDEV_FORMAT_TRY) {
 602                 cfg->try_fmt = *fmt;
 603                 return 0;
 604         }
 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_get_fmt(struct v4l2_subdev *sd,
 652                 struct v4l2_subdev_pad_config *cfg,
 653                 struct v4l2_subdev_format *format)
 654 {
 655         struct vs6624 *sensor = to_vs6624(sd);
 656
 657         if (format->pad)
 658                 return -EINVAL;
 659
 660         format->format = sensor->fmt;
 661         return 0;
 662 }
 663
 664 static int vs6624_g_parm(struct v4l2_subdev *sd, struct v4l2_streamparm *parms)
 665 {
 666         struct vs6624 *sensor = to_vs6624(sd);
 667         struct v4l2_captureparm *cp = &parms->parm.capture;
 668
 669         if (parms->type != V4L2_BUF_TYPE_VIDEO_CAPTURE)
 670                 return -EINVAL;
 671
 672         memset(cp, 0, sizeof(*cp));
 673         cp->capability = V4L2_CAP_TIMEPERFRAME;
 674         cp->timeperframe.numerator = sensor->frame_rate.denominator;
 675         cp->timeperframe.denominator = sensor->frame_rate.numerator;
 676         return 0;
 677 }
 678
 679 static int vs6624_s_parm(struct v4l2_subdev *sd, struct v4l2_streamparm *parms)
 680 {
 681         struct vs6624 *sensor = to_vs6624(sd);
 682         struct v4l2_captureparm *cp = &parms->parm.capture;
 683         struct v4l2_fract *tpf = &cp->timeperframe;
 684
 685         if (parms->type != V4L2_BUF_TYPE_VIDEO_CAPTURE)
 686                 return -EINVAL;
 687         if (cp->extendedmode != 0)
 688                 return -EINVAL;
 689
 690         if (tpf->numerator == 0 || tpf->denominator == 0
 691                 || (tpf->denominator > tpf->numerator * MAX_FRAME_RATE)) {
 692                 /* reset to max frame rate */
 693                 tpf->numerator = 1;
 694                 tpf->denominator = MAX_FRAME_RATE;
 695         }
 696         sensor->frame_rate.numerator = tpf->denominator;
 697         sensor->frame_rate.denominator = tpf->numerator;
 698         vs6624_write(sd, VS6624_DISABLE_FR_DAMPER, 0x0);
 699         vs6624_write(sd, VS6624_FR_NUM_MSB,
 700                         sensor->frame_rate.numerator >> 8);
 701         vs6624_write(sd, VS6624_FR_NUM_LSB,
 702                         sensor->frame_rate.numerator & 0xFF);
 703         vs6624_write(sd, VS6624_FR_DEN,
 704                         sensor->frame_rate.denominator & 0xFF);
 705         return 0;
 706 }
 707
 708 static int vs6624_s_stream(struct v4l2_subdev *sd, int enable)
 709 {
 710         if (enable)
 711                 vs6624_write(sd, VS6624_USER_CMD, 0x2);
 712         else
 713                 vs6624_write(sd, VS6624_USER_CMD, 0x4);
 714         udelay(100);
 715         return 0;
 716 }
 717
 718 #ifdef CONFIG_VIDEO_ADV_DEBUG
 719 static int vs6624_g_register(struct v4l2_subdev *sd, struct v4l2_dbg_register *reg)
 720 {
 721         reg->val = vs6624_read(sd, reg->reg & 0xffff);
 722         reg->size = 1;
 723         return 0;
 724 }
 725
 726 static int vs6624_s_register(struct v4l2_subdev *sd, const struct v4l2_dbg_register *reg)
 727 {
 728         vs6624_write(sd, reg->reg & 0xffff, reg->val & 0xff);
 729         return 0;
 730 }
 731 #endif
 732
 733 static const struct v4l2_ctrl_ops vs6624_ctrl_ops = {
 734         .s_ctrl = vs6624_s_ctrl,
 735 };
 736
 737 static const struct v4l2_subdev_core_ops vs6624_core_ops = {
 738 #ifdef CONFIG_VIDEO_ADV_DEBUG
 739         .g_register = vs6624_g_register,
 740         .s_register = vs6624_s_register,
 741 #endif
 742 };
 743
 744 static const struct v4l2_subdev_video_ops vs6624_video_ops = {
 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_pad_ops vs6624_pad_ops = {
 751         .enum_mbus_code = vs6624_enum_mbus_code,
 752         .get_fmt = vs6624_get_fmt,
 753         .set_fmt = vs6624_set_fmt,
 754 };
 755
 756 static const struct v4l2_subdev_ops vs6624_ops = {
 757         .core = &vs6624_core_ops,
 758         .video = &vs6624_video_ops,
 759         .pad = &vs6624_pad_ops,
 760 };
 761
 762 static int vs6624_probe(struct i2c_client *client,
 763                         const struct i2c_device_id *id)
 764 {
 765         struct vs6624 *sensor;
 766         struct v4l2_subdev *sd;
 767         struct v4l2_ctrl_handler *hdl;
 768         const unsigned *ce;
 769         int ret;
 770
 771         /* Check if the adapter supports the needed features */
 772         if (!i2c_check_functionality(client->adapter, I2C_FUNC_I2C))
 773                 return -EIO;
 774
 775         ce = client->dev.platform_data;
 776         if (ce == NULL)
 777                 return -EINVAL;
 778
 779         ret = devm_gpio_request_one(&client->dev, *ce, GPIOF_OUT_INIT_HIGH,
 780                                     "VS6624 Chip Enable");
 781         if (ret) {
 782                 v4l_err(client, "failed to request GPIO %d\n", *ce);
 783                 return ret;
 784         }
 785         /* wait 100ms before any further i2c writes are performed */
 786         mdelay(100);
 787
 788         sensor = devm_kzalloc(&client->dev, sizeof(*sensor), GFP_KERNEL);
 789         if (sensor == NULL)
 790                 return -ENOMEM;
 791
 792         sd = &sensor->sd;
 793         v4l2_i2c_subdev_init(sd, client, &vs6624_ops);
 794
 795         vs6624_writeregs(sd, vs6624_p1);
 796         vs6624_write(sd, VS6624_MICRO_EN, 0x2);
 797         vs6624_write(sd, VS6624_DIO_EN, 0x1);
 798         mdelay(10);
 799         vs6624_writeregs(sd, vs6624_p2);
 800
 801         vs6624_writeregs(sd, vs6624_default);
 802         vs6624_write(sd, VS6624_HSYNC_SETUP, 0xF);
 803         vs6624_writeregs(sd, vs6624_run_setup);
 804
 805         /* set frame rate */
 806         sensor->frame_rate.numerator = MAX_FRAME_RATE;
 807         sensor->frame_rate.denominator = 1;
 808         vs6624_write(sd, VS6624_DISABLE_FR_DAMPER, 0x0);
 809         vs6624_write(sd, VS6624_FR_NUM_MSB,
 810                         sensor->frame_rate.numerator >> 8);
 811         vs6624_write(sd, VS6624_FR_NUM_LSB,
 812                         sensor->frame_rate.numerator & 0xFF);
 813         vs6624_write(sd, VS6624_FR_DEN,
 814                         sensor->frame_rate.denominator & 0xFF);
 815
 816         sensor->fmt = vs6624_default_fmt;
 817         sensor->ce_pin = *ce;
 818
 819         v4l_info(client, "chip found @ 0x%02x (%s)\n",
 820                         client->addr << 1, client->adapter->name);
 821
 822         hdl = &sensor->hdl;
 823         v4l2_ctrl_handler_init(hdl, 4);
 824         v4l2_ctrl_new_std(hdl, &vs6624_ctrl_ops,
 825                         V4L2_CID_CONTRAST, 0, 0xFF, 1, 0x87);
 826         v4l2_ctrl_new_std(hdl, &vs6624_ctrl_ops,
 827                         V4L2_CID_SATURATION, 0, 0xFF, 1, 0x78);
 828         v4l2_ctrl_new_std(hdl, &vs6624_ctrl_ops,
 829                         V4L2_CID_HFLIP, 0, 1, 1, 0);
 830         v4l2_ctrl_new_std(hdl, &vs6624_ctrl_ops,
 831                         V4L2_CID_VFLIP, 0, 1, 1, 0);
 832         /* hook the control handler into the driver */
 833         sd->ctrl_handler = hdl;
 834         if (hdl->error) {
 835                 int err = hdl->error;
 836
 837                 v4l2_ctrl_handler_free(hdl);
 838                 return err;
 839         }
 840
 841         /* initialize the hardware to the default control values */
 842         ret = v4l2_ctrl_handler_setup(hdl);
 843         if (ret)
 844                 v4l2_ctrl_handler_free(hdl);
 845         return ret;
 846 }
 847
 848 static int vs6624_remove(struct i2c_client *client)
 849 {
 850         struct v4l2_subdev *sd = i2c_get_clientdata(client);
 851
 852         v4l2_device_unregister_subdev(sd);
 853         v4l2_ctrl_handler_free(sd->ctrl_handler);
 854         return 0;
 855 }
 856
 857 static const struct i2c_device_id vs6624_id[] = {
 858         {"vs6624", 0},
 859         {},
 860 };
 861
 862 MODULE_DEVICE_TABLE(i2c, vs6624_id);
 863
 864 static struct i2c_driver vs6624_driver = {
 865         .driver = {
 866                 .owner  = THIS_MODULE,
 867                 .name   = "vs6624",
 868         },
 869         .probe          = vs6624_probe,
 870         .remove         = vs6624_remove,
 871         .id_table       = vs6624_id,
 872 };
 873
 874 module_i2c_driver(vs6624_driver);
 875
 876 MODULE_DESCRIPTION("VS6624 sensor driver");
 877 MODULE_AUTHOR("Scott Jiang <Scott.Jiang.Linux@gmail.com>");
 878 MODULE_LICENSE("GPL v2");