linux/drivers/media/i2c/vs6624.c
<<
>>
Prefs
   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
  52struct 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
  60static 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
  78static 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
  86static 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
 412static const u16 vs6624_p2[] = {
 413        0x806f, 0x01,
 414        0x058c, 0x01,
 415        0x0000, 0x00,
 416};
 417
 418static 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
 460static 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
 498static inline struct vs6624 *to_vs6624(struct v4l2_subdev *sd)
 499{
 500        return container_of(sd, struct vs6624, sd);
 501}
 502static inline struct v4l2_subdev *to_sd(struct v4l2_ctrl *ctrl)
 503{
 504        return &container_of(ctrl->handler, struct vs6624, hdl)->sd;
 505}
 506
 507static 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
 520static 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
 533static 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
 547static 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
 571static 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
 581static 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
 607static 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
 662static 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
 671static 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
 686static 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
 715static 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
 725static 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
 738static 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
 751static int vs6624_s_register(struct v4l2_subdev *sd, const 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
 764static const struct v4l2_ctrl_ops vs6624_ctrl_ops = {
 765        .s_ctrl = vs6624_s_ctrl,
 766};
 767
 768static 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
 776static 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
 786static const struct v4l2_subdev_ops vs6624_ops = {
 787        .core = &vs6624_core_ops,
 788        .video = &vs6624_video_ops,
 789};
 790
 791static int 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
 884static int 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
 896static const struct i2c_device_id vs6624_id[] = {
 897        {"vs6624", 0},
 898        {},
 899};
 900
 901MODULE_DEVICE_TABLE(i2c, vs6624_id);
 902
 903static struct i2c_driver vs6624_driver = {
 904        .driver = {
 905                .owner  = THIS_MODULE,
 906                .name   = "vs6624",
 907        },
 908        .probe          = vs6624_probe,
 909        .remove         = vs6624_remove,
 910        .id_table       = vs6624_id,
 911};
 912
 913module_i2c_driver(vs6624_driver);
 914
 915MODULE_DESCRIPTION("VS6624 sensor driver");
 916MODULE_AUTHOR("Scott Jiang <Scott.Jiang.Linux@gmail.com>");
 917MODULE_LICENSE("GPL v2");
 918