drivers/media/i2c/vs6624.c

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