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