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 #ifdef CONFIG_VIDEO_ADV_DEBUG
 507 static int vs6624_read(struct v4l2_subdev *sd, u16 index)
 508 {
 509         struct i2c_client *client = v4l2_get_subdevdata(sd);
 510         u8 buf[2];
 511
 512         buf[0] = index >> 8;
 513         buf[1] = index;
 514         i2c_master_send(client, buf, 2);
 515         i2c_master_recv(client, buf, 1);
 516
 517         return buf[0];
 518 }
 519 #endif
 520
 521 static int vs6624_write(struct v4l2_subdev *sd, u16 index,
 522                                 u8 value)
 523 {
 524         struct i2c_client *client = v4l2_get_subdevdata(sd);
 525         u8 buf[3];
 526
 527         buf[0] = index >> 8;
 528         buf[1] = index;
 529         buf[2] = value;
 530
 531         return i2c_master_send(client, buf, 3);
 532 }
 533
 534 static int vs6624_writeregs(struct v4l2_subdev *sd, const u16 *regs)
 535 {
 536         u16 reg;
 537         u8 data;
 538
 539         while (*regs != 0x00) {
 540                 reg = *regs++;
 541                 data = *regs++;
 542
 543                 vs6624_write(sd, reg, data);
 544         }
 545         return 0;
 546 }
 547
 548 static int vs6624_s_ctrl(struct v4l2_ctrl *ctrl)
 549 {
 550         struct v4l2_subdev *sd = to_sd(ctrl);
 551
 552         switch (ctrl->id) {
 553         case V4L2_CID_CONTRAST:
 554                 vs6624_write(sd, VS6624_CONTRAST0, ctrl->val);
 555                 break;
 556         case V4L2_CID_SATURATION:
 557                 vs6624_write(sd, VS6624_SATURATION0, ctrl->val);
 558                 break;
 559         case V4L2_CID_HFLIP:
 560                 vs6624_write(sd, VS6624_HMIRROR0, ctrl->val);
 561                 break;
 562         case V4L2_CID_VFLIP:
 563                 vs6624_write(sd, VS6624_VFLIP0, ctrl->val);
 564                 break;
 565         default:
 566                 return -EINVAL;
 567         }
 568
 569         return 0;
 570 }
 571
 572 static int vs6624_enum_mbus_fmt(struct v4l2_subdev *sd, unsigned index,
 573                                 enum v4l2_mbus_pixelcode *code)
 574 {
 575         if (index >= ARRAY_SIZE(vs6624_formats))
 576                 return -EINVAL;
 577
 578         *code = vs6624_formats[index].mbus_code;
 579         return 0;
 580 }
 581
 582 static int vs6624_try_mbus_fmt(struct v4l2_subdev *sd,
 583                                 struct v4l2_mbus_framefmt *fmt)
 584 {
 585         int index;
 586
 587         for (index = 0; index < ARRAY_SIZE(vs6624_formats); index++)
 588                 if (vs6624_formats[index].mbus_code == fmt->code)
 589                         break;
 590         if (index >= ARRAY_SIZE(vs6624_formats)) {
 591                 /* default to first format */
 592                 index = 0;
 593                 fmt->code = vs6624_formats[0].mbus_code;
 594         }
 595
 596         /* sensor mode is VGA */
 597         if (fmt->width > VGA_WIDTH)
 598                 fmt->width = VGA_WIDTH;
 599         if (fmt->height > VGA_HEIGHT)
 600                 fmt->height = VGA_HEIGHT;
 601         fmt->width = fmt->width & (~3);
 602         fmt->height = fmt->height & (~3);
 603         fmt->field = V4L2_FIELD_NONE;
 604         fmt->colorspace = vs6624_formats[index].colorspace;
 605         return 0;
 606 }
 607
 608 static int vs6624_s_mbus_fmt(struct v4l2_subdev *sd,
 609                                 struct v4l2_mbus_framefmt *fmt)
 610 {
 611         struct vs6624 *sensor = to_vs6624(sd);
 612         int ret;
 613
 614         ret = vs6624_try_mbus_fmt(sd, fmt);
 615         if (ret)
 616                 return ret;
 617
 618         /* set image format */
 619         switch (fmt->code) {
 620         case V4L2_MBUS_FMT_UYVY8_2X8:
 621                 vs6624_write(sd, VS6624_IMG_FMT0, 0x0);
 622                 vs6624_write(sd, VS6624_YUV_SETUP, 0x1);
 623                 break;
 624         case V4L2_MBUS_FMT_YUYV8_2X8:
 625                 vs6624_write(sd, VS6624_IMG_FMT0, 0x0);
 626                 vs6624_write(sd, VS6624_YUV_SETUP, 0x3);
 627                 break;
 628         case V4L2_MBUS_FMT_RGB565_2X8_LE:
 629                 vs6624_write(sd, VS6624_IMG_FMT0, 0x4);
 630                 vs6624_write(sd, VS6624_RGB_SETUP, 0x0);
 631                 break;
 632         default:
 633                 return -EINVAL;
 634         }
 635
 636         /* set image size */
 637         if ((fmt->width == VGA_WIDTH) && (fmt->height == VGA_HEIGHT))
 638                 vs6624_write(sd, VS6624_IMAGE_SIZE0, 0x2);
 639         else if ((fmt->width == QVGA_WIDTH) && (fmt->height == QVGA_HEIGHT))
 640                 vs6624_write(sd, VS6624_IMAGE_SIZE0, 0x4);
 641         else if ((fmt->width == QQVGA_WIDTH) && (fmt->height == QQVGA_HEIGHT))
 642                 vs6624_write(sd, VS6624_IMAGE_SIZE0, 0x6);
 643         else if ((fmt->width == CIF_WIDTH) && (fmt->height == CIF_HEIGHT))
 644                 vs6624_write(sd, VS6624_IMAGE_SIZE0, 0x3);
 645         else if ((fmt->width == QCIF_WIDTH) && (fmt->height == QCIF_HEIGHT))
 646                 vs6624_write(sd, VS6624_IMAGE_SIZE0, 0x5);
 647         else if ((fmt->width == QQCIF_WIDTH) && (fmt->height == QQCIF_HEIGHT))
 648                 vs6624_write(sd, VS6624_IMAGE_SIZE0, 0x7);
 649         else {
 650                 vs6624_write(sd, VS6624_IMAGE_SIZE0, 0x8);
 651                 vs6624_write(sd, VS6624_MAN_HSIZE0_MSB, fmt->width >> 8);
 652                 vs6624_write(sd, VS6624_MAN_HSIZE0_LSB, fmt->width & 0xFF);
 653                 vs6624_write(sd, VS6624_MAN_VSIZE0_MSB, fmt->height >> 8);
 654                 vs6624_write(sd, VS6624_MAN_VSIZE0_LSB, fmt->height & 0xFF);
 655                 vs6624_write(sd, VS6624_CROP_CTRL0, 0x1);
 656         }
 657
 658         sensor->fmt = *fmt;
 659
 660         return 0;
 661 }
 662
 663 static int vs6624_g_mbus_fmt(struct v4l2_subdev *sd,
 664                                 struct v4l2_mbus_framefmt *fmt)
 665 {
 666         struct vs6624 *sensor = to_vs6624(sd);
 667
 668         *fmt = sensor->fmt;
 669         return 0;
 670 }
 671
 672 static int vs6624_g_parm(struct v4l2_subdev *sd, struct v4l2_streamparm *parms)
 673 {
 674         struct vs6624 *sensor = to_vs6624(sd);
 675         struct v4l2_captureparm *cp = &parms->parm.capture;
 676
 677         if (parms->type != V4L2_BUF_TYPE_VIDEO_CAPTURE)
 678                 return -EINVAL;
 679
 680         memset(cp, 0, sizeof(*cp));
 681         cp->capability = V4L2_CAP_TIMEPERFRAME;
 682         cp->timeperframe.numerator = sensor->frame_rate.denominator;
 683         cp->timeperframe.denominator = sensor->frame_rate.numerator;
 684         return 0;
 685 }
 686
 687 static int vs6624_s_parm(struct v4l2_subdev *sd, struct v4l2_streamparm *parms)
 688 {
 689         struct vs6624 *sensor = to_vs6624(sd);
 690         struct v4l2_captureparm *cp = &parms->parm.capture;
 691         struct v4l2_fract *tpf = &cp->timeperframe;
 692
 693         if (parms->type != V4L2_BUF_TYPE_VIDEO_CAPTURE)
 694                 return -EINVAL;
 695         if (cp->extendedmode != 0)
 696                 return -EINVAL;
 697
 698         if (tpf->numerator == 0 || tpf->denominator == 0
 699                 || (tpf->denominator > tpf->numerator * MAX_FRAME_RATE)) {
 700                 /* reset to max frame rate */
 701                 tpf->numerator = 1;
 702                 tpf->denominator = MAX_FRAME_RATE;
 703         }
 704         sensor->frame_rate.numerator = tpf->denominator;
 705         sensor->frame_rate.denominator = tpf->numerator;
 706         vs6624_write(sd, VS6624_DISABLE_FR_DAMPER, 0x0);
 707         vs6624_write(sd, VS6624_FR_NUM_MSB,
 708                         sensor->frame_rate.numerator >> 8);
 709         vs6624_write(sd, VS6624_FR_NUM_LSB,
 710                         sensor->frame_rate.numerator & 0xFF);
 711         vs6624_write(sd, VS6624_FR_DEN,
 712                         sensor->frame_rate.denominator & 0xFF);
 713         return 0;
 714 }
 715
 716 static int vs6624_s_stream(struct v4l2_subdev *sd, int enable)
 717 {
 718         if (enable)
 719                 vs6624_write(sd, VS6624_USER_CMD, 0x2);
 720         else
 721                 vs6624_write(sd, VS6624_USER_CMD, 0x4);
 722         udelay(100);
 723         return 0;
 724 }
 725
 726 #ifdef CONFIG_VIDEO_ADV_DEBUG
 727 static int vs6624_g_register(struct v4l2_subdev *sd, struct v4l2_dbg_register *reg)
 728 {
 729         reg->val = vs6624_read(sd, reg->reg & 0xffff);
 730         reg->size = 1;
 731         return 0;
 732 }
 733
 734 static int vs6624_s_register(struct v4l2_subdev *sd, const struct v4l2_dbg_register *reg)
 735 {
 736         vs6624_write(sd, reg->reg & 0xffff, reg->val & 0xff);
 737         return 0;
 738 }
 739 #endif
 740
 741 static const struct v4l2_ctrl_ops vs6624_ctrl_ops = {
 742         .s_ctrl = vs6624_s_ctrl,
 743 };
 744
 745 static const struct v4l2_subdev_core_ops vs6624_core_ops = {
 746 #ifdef CONFIG_VIDEO_ADV_DEBUG
 747         .g_register = vs6624_g_register,
 748         .s_register = vs6624_s_register,
 749 #endif
 750 };
 751
 752 static const struct v4l2_subdev_video_ops vs6624_video_ops = {
 753         .enum_mbus_fmt = vs6624_enum_mbus_fmt,
 754         .try_mbus_fmt = vs6624_try_mbus_fmt,
 755         .s_mbus_fmt = vs6624_s_mbus_fmt,
 756         .g_mbus_fmt = vs6624_g_mbus_fmt,
 757         .s_parm = vs6624_s_parm,
 758         .g_parm = vs6624_g_parm,
 759         .s_stream = vs6624_s_stream,
 760 };
 761
 762 static const struct v4l2_subdev_ops vs6624_ops = {
 763         .core = &vs6624_core_ops,
 764         .video = &vs6624_video_ops,
 765 };
 766
 767 static int vs6624_probe(struct i2c_client *client,
 768                         const struct i2c_device_id *id)
 769 {
 770         struct vs6624 *sensor;
 771         struct v4l2_subdev *sd;
 772         struct v4l2_ctrl_handler *hdl;
 773         const unsigned *ce;
 774         int ret;
 775
 776         /* Check if the adapter supports the needed features */
 777         if (!i2c_check_functionality(client->adapter, I2C_FUNC_I2C))
 778                 return -EIO;
 779
 780         ce = client->dev.platform_data;
 781         if (ce == NULL)
 782                 return -EINVAL;
 783
 784         ret = devm_gpio_request_one(&client->dev, *ce, GPIOF_OUT_INIT_HIGH,
 785                                     "VS6624 Chip Enable");
 786         if (ret) {
 787                 v4l_err(client, "failed to request GPIO %d\n", *ce);
 788                 return ret;
 789         }
 790         /* wait 100ms before any further i2c writes are performed */
 791         mdelay(100);
 792
 793         sensor = devm_kzalloc(&client->dev, sizeof(*sensor), GFP_KERNEL);
 794         if (sensor == NULL)
 795                 return -ENOMEM;
 796
 797         sd = &sensor->sd;
 798         v4l2_i2c_subdev_init(sd, client, &vs6624_ops);
 799
 800         vs6624_writeregs(sd, vs6624_p1);
 801         vs6624_write(sd, VS6624_MICRO_EN, 0x2);
 802         vs6624_write(sd, VS6624_DIO_EN, 0x1);
 803         mdelay(10);
 804         vs6624_writeregs(sd, vs6624_p2);
 805
 806         vs6624_writeregs(sd, vs6624_default);
 807         vs6624_write(sd, VS6624_HSYNC_SETUP, 0xF);
 808         vs6624_writeregs(sd, vs6624_run_setup);
 809
 810         /* set frame rate */
 811         sensor->frame_rate.numerator = MAX_FRAME_RATE;
 812         sensor->frame_rate.denominator = 1;
 813         vs6624_write(sd, VS6624_DISABLE_FR_DAMPER, 0x0);
 814         vs6624_write(sd, VS6624_FR_NUM_MSB,
 815                         sensor->frame_rate.numerator >> 8);
 816         vs6624_write(sd, VS6624_FR_NUM_LSB,
 817                         sensor->frame_rate.numerator & 0xFF);
 818         vs6624_write(sd, VS6624_FR_DEN,
 819                         sensor->frame_rate.denominator & 0xFF);
 820
 821         sensor->fmt = vs6624_default_fmt;
 822         sensor->ce_pin = *ce;
 823
 824         v4l_info(client, "chip found @ 0x%02x (%s)\n",
 825                         client->addr << 1, client->adapter->name);
 826
 827         hdl = &sensor->hdl;
 828         v4l2_ctrl_handler_init(hdl, 4);
 829         v4l2_ctrl_new_std(hdl, &vs6624_ctrl_ops,
 830                         V4L2_CID_CONTRAST, 0, 0xFF, 1, 0x87);
 831         v4l2_ctrl_new_std(hdl, &vs6624_ctrl_ops,
 832                         V4L2_CID_SATURATION, 0, 0xFF, 1, 0x78);
 833         v4l2_ctrl_new_std(hdl, &vs6624_ctrl_ops,
 834                         V4L2_CID_HFLIP, 0, 1, 1, 0);
 835         v4l2_ctrl_new_std(hdl, &vs6624_ctrl_ops,
 836                         V4L2_CID_VFLIP, 0, 1, 1, 0);
 837         /* hook the control handler into the driver */
 838         sd->ctrl_handler = hdl;
 839         if (hdl->error) {
 840                 int err = hdl->error;
 841
 842                 v4l2_ctrl_handler_free(hdl);
 843                 return err;
 844         }
 845
 846         /* initialize the hardware to the default control values */
 847         ret = v4l2_ctrl_handler_setup(hdl);
 848         if (ret)
 849                 v4l2_ctrl_handler_free(hdl);
 850         return ret;
 851 }
 852
 853 static int vs6624_remove(struct i2c_client *client)
 854 {
 855         struct v4l2_subdev *sd = i2c_get_clientdata(client);
 856
 857         v4l2_device_unregister_subdev(sd);
 858         v4l2_ctrl_handler_free(sd->ctrl_handler);
 859         return 0;
 860 }
 861
 862 static const struct i2c_device_id vs6624_id[] = {
 863         {"vs6624", 0},
 864         {},
 865 };
 866
 867 MODULE_DEVICE_TABLE(i2c, vs6624_id);
 868
 869 static struct i2c_driver vs6624_driver = {
 870         .driver = {
 871                 .owner  = THIS_MODULE,
 872                 .name   = "vs6624",
 873         },
 874         .probe          = vs6624_probe,
 875         .remove         = vs6624_remove,
 876         .id_table       = vs6624_id,
 877 };
 878
 879 module_i2c_driver(vs6624_driver);
 880
 881 MODULE_DESCRIPTION("VS6624 sensor driver");
 882 MODULE_AUTHOR("Scott Jiang <Scott.Jiang.Linux@gmail.com>");
 883 MODULE_LICENSE("GPL v2");