drivers/media/i2c/ov9650.c

   1 /*
   2  * Omnivision OV9650/OV9652 CMOS Image Sensor driver
   3  *
   4  * Copyright (C) 2013, Sylwester Nawrocki <sylvester.nawrocki@gmail.com>
   5  *
   6  * Register definitions and initial settings based on a driver written
   7  * by Vladimir Fonov.
   8  * Copyright (c) 2010, Vladimir Fonov
   9  *
  10  * This program is free software; you can redistribute it and/or modify
  11  * it under the terms of the GNU General Public License version 2 as
  12  * published by the Free Software Foundation.
  13  */
  14 #include <linux/delay.h>
  15 #include <linux/gpio.h>
  16 #include <linux/i2c.h>
  17 #include <linux/kernel.h>
  18 #include <linux/media.h>
  19 #include <linux/module.h>
  20 #include <linux/ratelimit.h>
  21 #include <linux/slab.h>
  22 #include <linux/string.h>
  23 #include <linux/videodev2.h>
  24
  25 #include <media/media-entity.h>
  26 #include <media/v4l2-async.h>
  27 #include <media/v4l2-ctrls.h>
  28 #include <media/v4l2-device.h>
  29 #include <media/v4l2-event.h>
  30 #include <media/v4l2-image-sizes.h>
  31 #include <media/v4l2-subdev.h>
  32 #include <media/v4l2-mediabus.h>
  33 #include <media/i2c/ov9650.h>
  34
  35 static int debug;
  36 module_param(debug, int, 0644);
  37 MODULE_PARM_DESC(debug, "Debug level (0-2)");
  38
  39 #define DRIVER_NAME "OV9650"
  40
  41 /*
  42  * OV9650/OV9652 register definitions
  43  */
  44 #define REG_GAIN                0x00    /* Gain control, AGC[7:0] */
  45 #define REG_BLUE                0x01    /* AWB - Blue chanel gain */
  46 #define REG_RED                 0x02    /* AWB - Red chanel gain */
  47 #define REG_VREF                0x03    /* [7:6] - AGC[9:8], [5:3]/[2:0] */
  48 #define  VREF_GAIN_MASK         0xc0    /* - VREF end/start low 3 bits */
  49 #define REG_COM1                0x04
  50 #define  COM1_CCIR656           0x40
  51 #define REG_B_AVE               0x05
  52 #define REG_GB_AVE              0x06
  53 #define REG_GR_AVE              0x07
  54 #define REG_R_AVE               0x08
  55 #define REG_COM2                0x09
  56 #define REG_PID                 0x0a    /* Product ID MSB */
  57 #define REG_VER                 0x0b    /* Product ID LSB */
  58 #define REG_COM3                0x0c
  59 #define  COM3_SWAP              0x40
  60 #define  COM3_VARIOPIXEL1       0x04
  61 #define REG_COM4                0x0d    /* Vario Pixels  */
  62 #define  COM4_VARIOPIXEL2       0x80
  63 #define REG_COM5                0x0e    /* System clock options */
  64 #define  COM5_SLAVE_MODE        0x10
  65 #define  COM5_SYSTEMCLOCK48MHZ  0x80
  66 #define REG_COM6                0x0f    /* HREF & ADBLC options */
  67 #define REG_AECH                0x10    /* Exposure value, AEC[9:2] */
  68 #define REG_CLKRC               0x11    /* Clock control */
  69 #define  CLK_EXT                0x40    /* Use external clock directly */
  70 #define  CLK_SCALE              0x3f    /* Mask for internal clock scale */
  71 #define REG_COM7                0x12    /* SCCB reset, output format */
  72 #define  COM7_RESET             0x80
  73 #define  COM7_FMT_MASK          0x38
  74 #define  COM7_FMT_VGA           0x40
  75 #define  COM7_FMT_CIF           0x20
  76 #define  COM7_FMT_QVGA          0x10
  77 #define  COM7_FMT_QCIF          0x08
  78 #define  COM7_RGB               0x04
  79 #define  COM7_YUV               0x00
  80 #define  COM7_BAYER             0x01
  81 #define  COM7_PBAYER            0x05
  82 #define REG_COM8                0x13    /* AGC/AEC options */
  83 #define  COM8_FASTAEC           0x80    /* Enable fast AGC/AEC */
  84 #define  COM8_AECSTEP           0x40    /* Unlimited AEC step size */
  85 #define  COM8_BFILT             0x20    /* Band filter enable */
  86 #define  COM8_AGC               0x04    /* Auto gain enable */
  87 #define  COM8_AWB               0x02    /* White balance enable */
  88 #define  COM8_AEC               0x01    /* Auto exposure enable */
  89 #define REG_COM9                0x14    /* Gain ceiling */
  90 #define  COM9_GAIN_CEIL_MASK    0x70    /* */
  91 #define REG_COM10               0x15    /* PCLK, HREF, HSYNC signals polarity */
  92 #define  COM10_HSYNC            0x40    /* HSYNC instead of HREF */
  93 #define  COM10_PCLK_HB          0x20    /* Suppress PCLK on horiz blank */
  94 #define  COM10_HREF_REV         0x08    /* Reverse HREF */
  95 #define  COM10_VS_LEAD          0x04    /* VSYNC on clock leading edge */
  96 #define  COM10_VS_NEG           0x02    /* VSYNC negative */
  97 #define  COM10_HS_NEG           0x01    /* HSYNC negative */
  98 #define REG_HSTART              0x17    /* Horiz start high bits */
  99 #define REG_HSTOP               0x18    /* Horiz stop high bits */
 100 #define REG_VSTART              0x19    /* Vert start high bits */
 101 #define REG_VSTOP               0x1a    /* Vert stop high bits */
 102 #define REG_PSHFT               0x1b    /* Pixel delay after HREF */
 103 #define REG_MIDH                0x1c    /* Manufacturer ID MSB */
 104 #define REG_MIDL                0x1d    /* Manufufacturer ID LSB */
 105 #define REG_MVFP                0x1e    /* Image mirror/flip */
 106 #define  MVFP_MIRROR            0x20    /* Mirror image */
 107 #define  MVFP_FLIP              0x10    /* Vertical flip */
 108 #define REG_BOS                 0x20    /* B channel Offset */
 109 #define REG_GBOS                0x21    /* Gb channel Offset */
 110 #define REG_GROS                0x22    /* Gr channel Offset */
 111 #define REG_ROS                 0x23    /* R channel Offset */
 112 #define REG_AEW                 0x24    /* AGC upper limit */
 113 #define REG_AEB                 0x25    /* AGC lower limit */
 114 #define REG_VPT                 0x26    /* AGC/AEC fast mode op region */
 115 #define REG_BBIAS               0x27    /* B channel output bias */
 116 #define REG_GBBIAS              0x28    /* Gb channel output bias */
 117 #define REG_GRCOM               0x29    /* Analog BLC & regulator */
 118 #define REG_EXHCH               0x2a    /* Dummy pixel insert MSB */
 119 #define REG_EXHCL               0x2b    /* Dummy pixel insert LSB */
 120 #define REG_RBIAS               0x2c    /* R channel output bias */
 121 #define REG_ADVFL               0x2d    /* LSB of dummy line insert */
 122 #define REG_ADVFH               0x2e    /* MSB of dummy line insert */
 123 #define REG_YAVE                0x2f    /* Y/G channel average value */
 124 #define REG_HSYST               0x30    /* HSYNC rising edge delay LSB*/
 125 #define REG_HSYEN               0x31    /* HSYNC falling edge delay LSB*/
 126 #define REG_HREF                0x32    /* HREF pieces */
 127 #define REG_CHLF                0x33    /* reserved */
 128 #define REG_ADC                 0x37    /* reserved */
 129 #define REG_ACOM                0x38    /* reserved */
 130 #define REG_OFON                0x39    /* Power down register */
 131 #define  OFON_PWRDN             0x08    /* Power down bit */
 132 #define REG_TSLB                0x3a    /* YUVU format */
 133 #define  TSLB_YUYV_MASK         0x0c    /* UYVY or VYUY - see com13 */
 134 #define REG_COM11               0x3b    /* Night mode, banding filter enable */
 135 #define  COM11_NIGHT            0x80    /* Night mode enable */
 136 #define  COM11_NMFR             0x60    /* Two bit NM frame rate */
 137 #define  COM11_BANDING          0x01    /* Banding filter */
 138 #define  COM11_AEC_REF_MASK     0x18    /* AEC reference area selection */
 139 #define REG_COM12               0x3c    /* HREF option, UV average */
 140 #define  COM12_HREF             0x80    /* HREF always */
 141 #define REG_COM13               0x3d    /* Gamma selection, Color matrix en. */
 142 #define  COM13_GAMMA            0x80    /* Gamma enable */
 143 #define  COM13_UVSAT            0x40    /* UV saturation auto adjustment */
 144 #define  COM13_UVSWAP           0x01    /* V before U - w/TSLB */
 145 #define REG_COM14               0x3e    /* Edge enhancement options */
 146 #define  COM14_EDGE_EN          0x02
 147 #define  COM14_EEF_X2           0x01
 148 #define REG_EDGE                0x3f    /* Edge enhancement factor */
 149 #define  EDGE_FACTOR_MASK       0x0f
 150 #define REG_COM15               0x40    /* Output range, RGB 555/565 */
 151 #define  COM15_R10F0            0x00    /* Data range 10 to F0 */
 152 #define  COM15_R01FE            0x80    /* 01 to FE */
 153 #define  COM15_R00FF            0xc0    /* 00 to FF */
 154 #define  COM15_RGB565           0x10    /* RGB565 output */
 155 #define  COM15_RGB555           0x30    /* RGB555 output */
 156 #define  COM15_SWAPRB           0x04    /* Swap R&B */
 157 #define REG_COM16               0x41    /* Color matrix coeff options */
 158 #define REG_COM17               0x42    /* Single frame out, banding filter */
 159 /* n = 1...9, 0x4f..0x57 */
 160 #define REG_MTX(__n)            (0x4f + (__n) - 1)
 161 #define REG_MTXS                0x58
 162 /* Lens Correction Option 1...5, __n = 0...5 */
 163 #define REG_LCC(__n)            (0x62 + (__n) - 1)
 164 #define  LCC5_LCC_ENABLE        0x01    /* LCC5, enable lens correction */
 165 #define  LCC5_LCC_COLOR         0x04
 166 #define REG_MANU                0x67    /* Manual U value */
 167 #define REG_MANV                0x68    /* Manual V value */
 168 #define REG_HV                  0x69    /* Manual banding filter MSB */
 169 #define REG_MBD                 0x6a    /* Manual banding filter value */
 170 #define REG_DBLV                0x6b    /* reserved */
 171 #define REG_GSP                 0x6c    /* Gamma curve */
 172 #define  GSP_LEN                15
 173 #define REG_GST                 0x7c    /* Gamma curve */
 174 #define  GST_LEN                15
 175 #define REG_COM21               0x8b
 176 #define REG_COM22               0x8c    /* Edge enhancement, denoising */
 177 #define  COM22_WHTPCOR          0x02    /* White pixel correction enable */
 178 #define  COM22_WHTPCOROPT       0x01    /* White pixel correction option */
 179 #define  COM22_DENOISE          0x10    /* White pixel correction option */
 180 #define REG_COM23               0x8d    /* Color bar test, color gain */
 181 #define  COM23_TEST_MODE        0x10
 182 #define REG_DBLC1               0x8f    /* Digital BLC */
 183 #define REG_DBLC_B              0x90    /* Digital BLC B channel offset */
 184 #define REG_DBLC_R              0x91    /* Digital BLC R channel offset */
 185 #define REG_DM_LNL              0x92    /* Dummy line low 8 bits */
 186 #define REG_DM_LNH              0x93    /* Dummy line high 8 bits */
 187 #define REG_LCCFB               0x9d    /* Lens Correction B channel */
 188 #define REG_LCCFR               0x9e    /* Lens Correction R channel */
 189 #define REG_DBLC_GB             0x9f    /* Digital BLC GB chan offset */
 190 #define REG_DBLC_GR             0xa0    /* Digital BLC GR chan offset */
 191 #define REG_AECHM               0xa1    /* Exposure value - bits AEC[15:10] */
 192 #define REG_BD50ST              0xa2    /* Banding filter value for 50Hz */
 193 #define REG_BD60ST              0xa3    /* Banding filter value for 60Hz */
 194 #define REG_NULL                0xff    /* Array end token */
 195
 196 #define DEF_CLKRC               0x80
 197
 198 #define OV965X_ID(_msb, _lsb)   ((_msb) << 8 | (_lsb))
 199 #define OV9650_ID               0x9650
 200 #define OV9652_ID               0x9652
 201
 202 struct ov965x_ctrls {
 203         struct v4l2_ctrl_handler handler;
 204         struct {
 205                 struct v4l2_ctrl *auto_exp;
 206                 struct v4l2_ctrl *exposure;
 207         };
 208         struct {
 209                 struct v4l2_ctrl *auto_wb;
 210                 struct v4l2_ctrl *blue_balance;
 211                 struct v4l2_ctrl *red_balance;
 212         };
 213         struct {
 214                 struct v4l2_ctrl *hflip;
 215                 struct v4l2_ctrl *vflip;
 216         };
 217         struct {
 218                 struct v4l2_ctrl *auto_gain;
 219                 struct v4l2_ctrl *gain;
 220         };
 221         struct v4l2_ctrl *brightness;
 222         struct v4l2_ctrl *saturation;
 223         struct v4l2_ctrl *sharpness;
 224         struct v4l2_ctrl *light_freq;
 225         u8 update;
 226 };
 227
 228 struct ov965x_framesize {
 229         u16 width;
 230         u16 height;
 231         u16 max_exp_lines;
 232         const u8 *regs;
 233 };
 234
 235 struct ov965x_interval {
 236         struct v4l2_fract interval;
 237         /* Maximum resolution for this interval */
 238         struct v4l2_frmsize_discrete size;
 239         u8 clkrc_div;
 240 };
 241
 242 enum gpio_id {
 243         GPIO_PWDN,
 244         GPIO_RST,
 245         NUM_GPIOS,
 246 };
 247
 248 struct ov965x {
 249         struct v4l2_subdev sd;
 250         struct media_pad pad;
 251         enum v4l2_mbus_type bus_type;
 252         int gpios[NUM_GPIOS];
 253         /* External master clock frequency */
 254         unsigned long mclk_frequency;
 255
 256         /* Protects the struct fields below */
 257         struct mutex lock;
 258
 259         struct i2c_client *client;
 260
 261         /* Exposure row interval in us */
 262         unsigned int exp_row_interval;
 263
 264         unsigned short id;
 265         const struct ov965x_framesize *frame_size;
 266         /* YUYV sequence (pixel format) control register */
 267         u8 tslb_reg;
 268         struct v4l2_mbus_framefmt format;
 269
 270         struct ov965x_ctrls ctrls;
 271         /* Pointer to frame rate control data structure */
 272         const struct ov965x_interval *fiv;
 273
 274         int streaming;
 275         int power;
 276
 277         u8 apply_frame_fmt;
 278 };
 279
 280 struct i2c_rv {
 281         u8 addr;
 282         u8 value;
 283 };
 284
 285 static const struct i2c_rv ov965x_init_regs[] = {
 286         { REG_COM2, 0x10 },     /* Set soft sleep mode */
 287         { REG_COM5, 0x00 },     /* System clock options */
 288         { REG_COM2, 0x01 },     /* Output drive, soft sleep mode */
 289         { REG_COM10, 0x00 },    /* Slave mode, HREF vs HSYNC, signals negate */
 290         { REG_EDGE, 0xa6 },     /* Edge enhancement treshhold and factor */
 291         { REG_COM16, 0x02 },    /* Color matrix coeff double option */
 292         { REG_COM17, 0x08 },    /* Single frame out, banding filter */
 293         { 0x16, 0x06 },
 294         { REG_CHLF, 0xc0 },     /* Reserved  */
 295         { 0x34, 0xbf },
 296         { 0xa8, 0x80 },
 297         { 0x96, 0x04 },
 298         { 0x8e, 0x00 },
 299         { REG_COM12, 0x77 },    /* HREF option, UV average  */
 300         { 0x8b, 0x06 },
 301         { 0x35, 0x91 },
 302         { 0x94, 0x88 },
 303         { 0x95, 0x88 },
 304         { REG_COM15, 0xc1 },    /* Output range, RGB 555/565 */
 305         { REG_GRCOM, 0x2f },    /* Analog BLC & regulator */
 306         { REG_COM6, 0x43 },     /* HREF & ADBLC options */
 307         { REG_COM8, 0xe5 },     /* AGC/AEC options */
 308         { REG_COM13, 0x90 },    /* Gamma selection, colour matrix, UV delay */
 309         { REG_HV, 0x80 },       /* Manual banding filter MSB  */
 310         { 0x5c, 0x96 },         /* Reserved up to 0xa5 */
 311         { 0x5d, 0x96 },
 312         { 0x5e, 0x10 },
 313         { 0x59, 0xeb },
 314         { 0x5a, 0x9c },
 315         { 0x5b, 0x55 },
 316         { 0x43, 0xf0 },
 317         { 0x44, 0x10 },
 318         { 0x45, 0x55 },
 319         { 0x46, 0x86 },
 320         { 0x47, 0x64 },
 321         { 0x48, 0x86 },
 322         { 0x5f, 0xe0 },
 323         { 0x60, 0x8c },
 324         { 0x61, 0x20 },
 325         { 0xa5, 0xd9 },
 326         { 0xa4, 0x74 },         /* reserved */
 327         { REG_COM23, 0x02 },    /* Color gain analog/_digital_ */
 328         { REG_COM8, 0xe7 },     /* Enable AEC, AWB, AEC */
 329         { REG_COM22, 0x23 },    /* Edge enhancement, denoising */
 330         { 0xa9, 0xb8 },
 331         { 0xaa, 0x92 },
 332         { 0xab, 0x0a },
 333         { REG_DBLC1, 0xdf },    /* Digital BLC */
 334         { REG_DBLC_B, 0x00 },   /* Digital BLC B chan offset */
 335         { REG_DBLC_R, 0x00 },   /* Digital BLC R chan offset */
 336         { REG_DBLC_GB, 0x00 },  /* Digital BLC GB chan offset */
 337         { REG_DBLC_GR, 0x00 },
 338         { REG_COM9, 0x3a },     /* Gain ceiling 16x */
 339         { REG_NULL, 0 }
 340 };
 341
 342 #define NUM_FMT_REGS 14
 343 /*
 344  * COM7,  COM3,  COM4, HSTART, HSTOP, HREF, VSTART, VSTOP, VREF,
 345  * EXHCH, EXHCL, ADC,  OCOM,   OFON
 346  */
 347 static const u8 frame_size_reg_addr[NUM_FMT_REGS] = {
 348         0x12, 0x0c, 0x0d, 0x17, 0x18, 0x32, 0x19, 0x1a, 0x03,
 349         0x2a, 0x2b, 0x37, 0x38, 0x39,
 350 };
 351
 352 static const u8 ov965x_sxga_regs[NUM_FMT_REGS] = {
 353         0x00, 0x00, 0x00, 0x1e, 0xbe, 0xbf, 0x01, 0x81, 0x12,
 354         0x10, 0x34, 0x81, 0x93, 0x51,
 355 };
 356
 357 static const u8 ov965x_vga_regs[NUM_FMT_REGS] = {
 358         0x40, 0x04, 0x80, 0x26, 0xc6, 0xed, 0x01, 0x3d, 0x00,
 359         0x10, 0x40, 0x91, 0x12, 0x43,
 360 };
 361
 362 /* Determined empirically. */
 363 static const u8 ov965x_qvga_regs[NUM_FMT_REGS] = {
 364         0x10, 0x04, 0x80, 0x25, 0xc5, 0xbf, 0x00, 0x80, 0x12,
 365         0x10, 0x40, 0x91, 0x12, 0x43,
 366 };
 367
 368 static const struct ov965x_framesize ov965x_framesizes[] = {
 369         {
 370                 .width          = SXGA_WIDTH,
 371                 .height         = SXGA_HEIGHT,
 372                 .regs           = ov965x_sxga_regs,
 373                 .max_exp_lines  = 1048,
 374         }, {
 375                 .width          = VGA_WIDTH,
 376                 .height         = VGA_HEIGHT,
 377                 .regs           = ov965x_vga_regs,
 378                 .max_exp_lines  = 498,
 379         }, {
 380                 .width          = QVGA_WIDTH,
 381                 .height         = QVGA_HEIGHT,
 382                 .regs           = ov965x_qvga_regs,
 383                 .max_exp_lines  = 248,
 384         },
 385 };
 386
 387 struct ov965x_pixfmt {
 388         u32 code;
 389         u32 colorspace;
 390         /* REG_TSLB value, only bits [3:2] may be set. */
 391         u8 tslb_reg;
 392 };
 393
 394 static const struct ov965x_pixfmt ov965x_formats[] = {
 395         { MEDIA_BUS_FMT_YUYV8_2X8, V4L2_COLORSPACE_JPEG, 0x00},
 396         { MEDIA_BUS_FMT_YVYU8_2X8, V4L2_COLORSPACE_JPEG, 0x04},
 397         { MEDIA_BUS_FMT_UYVY8_2X8, V4L2_COLORSPACE_JPEG, 0x0c},
 398         { MEDIA_BUS_FMT_VYUY8_2X8, V4L2_COLORSPACE_JPEG, 0x08},
 399 };
 400
 401 /*
 402  * This table specifies possible frame resolution and interval
 403  * combinations. Default CLKRC[5:0] divider values are valid
 404  * only for 24 MHz external clock frequency.
 405  */
 406 static struct ov965x_interval ov965x_intervals[] = {
 407         {{ 100, 625 }, { SXGA_WIDTH, SXGA_HEIGHT }, 0 },  /* 6.25 fps */
 408         {{ 10,  125 }, { VGA_WIDTH, VGA_HEIGHT },   1 },  /* 12.5 fps */
 409         {{ 10,  125 }, { QVGA_WIDTH, QVGA_HEIGHT }, 3 },  /* 12.5 fps */
 410         {{ 1,   25  }, { VGA_WIDTH, VGA_HEIGHT },   0 },  /* 25 fps */
 411         {{ 1,   25  }, { QVGA_WIDTH, QVGA_HEIGHT }, 1 },  /* 25 fps */
 412 };
 413
 414 static inline struct v4l2_subdev *ctrl_to_sd(struct v4l2_ctrl *ctrl)
 415 {
 416         return &container_of(ctrl->handler, struct ov965x, ctrls.handler)->sd;
 417 }
 418
 419 static inline struct ov965x *to_ov965x(struct v4l2_subdev *sd)
 420 {
 421         return container_of(sd, struct ov965x, sd);
 422 }
 423
 424 static int ov965x_read(struct i2c_client *client, u8 addr, u8 *val)
 425 {
 426         u8 buf = addr;
 427         struct i2c_msg msg = {
 428                 .addr = client->addr,
 429                 .flags = 0,
 430                 .len = 1,
 431                 .buf = &buf
 432         };
 433         int ret;
 434
 435         ret = i2c_transfer(client->adapter, &msg, 1);
 436         if (ret == 1) {
 437                 msg.flags = I2C_M_RD;
 438                 ret = i2c_transfer(client->adapter, &msg, 1);
 439
 440                 if (ret == 1)
 441                         *val = buf;
 442         }
 443
 444         v4l2_dbg(2, debug, client, "%s: 0x%02x @ 0x%02x. (%d)\n",
 445                  __func__, *val, addr, ret);
 446
 447         return ret == 1 ? 0 : ret;
 448 }
 449
 450 static int ov965x_write(struct i2c_client *client, u8 addr, u8 val)
 451 {
 452         u8 buf[2] = { addr, val };
 453
 454         int ret = i2c_master_send(client, buf, 2);
 455
 456         v4l2_dbg(2, debug, client, "%s: 0x%02x @ 0x%02X (%d)\n",
 457                  __func__, val, addr, ret);
 458
 459         return ret == 2 ? 0 : ret;
 460 }
 461
 462 static int ov965x_write_array(struct i2c_client *client,
 463                               const struct i2c_rv *regs)
 464 {
 465         int i, ret = 0;
 466
 467         for (i = 0; ret == 0 && regs[i].addr != REG_NULL; i++)
 468                 ret = ov965x_write(client, regs[i].addr, regs[i].value);
 469
 470         return ret;
 471 }
 472
 473 static int ov965x_set_default_gamma_curve(struct ov965x *ov965x)
 474 {
 475         static const u8 gamma_curve[] = {
 476                 /* Values taken from OV application note. */
 477                 0x40, 0x30, 0x4b, 0x60, 0x70, 0x70, 0x70, 0x70,
 478                 0x60, 0x60, 0x50, 0x48, 0x3a, 0x2e, 0x28, 0x22,
 479                 0x04, 0x07, 0x10, 0x28, 0x36, 0x44, 0x52, 0x60,
 480                 0x6c, 0x78, 0x8c, 0x9e, 0xbb, 0xd2, 0xe6
 481         };
 482         u8 addr = REG_GSP;
 483         unsigned int i;
 484
 485         for (i = 0; i < ARRAY_SIZE(gamma_curve); i++) {
 486                 int ret = ov965x_write(ov965x->client, addr, gamma_curve[i]);
 487
 488                 if (ret < 0)
 489                         return ret;
 490                 addr++;
 491         }
 492
 493         return 0;
 494 };
 495
 496 static int ov965x_set_color_matrix(struct ov965x *ov965x)
 497 {
 498         static const u8 mtx[] = {
 499                 /* MTX1..MTX9, MTXS */
 500                 0x3a, 0x3d, 0x03, 0x12, 0x26, 0x38, 0x40, 0x40, 0x40, 0x0d
 501         };
 502         u8 addr = REG_MTX(1);
 503         unsigned int i;
 504
 505         for (i = 0; i < ARRAY_SIZE(mtx); i++) {
 506                 int ret = ov965x_write(ov965x->client, addr, mtx[i]);
 507
 508                 if (ret < 0)
 509                         return ret;
 510                 addr++;
 511         }
 512
 513         return 0;
 514 }
 515
 516 static void ov965x_gpio_set(int gpio, int val)
 517 {
 518         if (gpio_is_valid(gpio))
 519                 gpio_set_value(gpio, val);
 520 }
 521
 522 static void __ov965x_set_power(struct ov965x *ov965x, int on)
 523 {
 524         if (on) {
 525                 ov965x_gpio_set(ov965x->gpios[GPIO_PWDN], 0);
 526                 ov965x_gpio_set(ov965x->gpios[GPIO_RST], 0);
 527                 msleep(25);
 528         } else {
 529                 ov965x_gpio_set(ov965x->gpios[GPIO_RST], 1);
 530                 ov965x_gpio_set(ov965x->gpios[GPIO_PWDN], 1);
 531         }
 532
 533         ov965x->streaming = 0;
 534 }
 535
 536 static int ov965x_s_power(struct v4l2_subdev *sd, int on)
 537 {
 538         struct ov965x *ov965x = to_ov965x(sd);
 539         struct i2c_client *client = ov965x->client;
 540         int ret = 0;
 541
 542         v4l2_dbg(1, debug, client, "%s: on: %d\n", __func__, on);
 543
 544         mutex_lock(&ov965x->lock);
 545         if (ov965x->power == !on) {
 546                 __ov965x_set_power(ov965x, on);
 547                 if (on) {
 548                         ret = ov965x_write_array(client,
 549                                                  ov965x_init_regs);
 550                         ov965x->apply_frame_fmt = 1;
 551                         ov965x->ctrls.update = 1;
 552                 }
 553         }
 554         if (!ret)
 555                 ov965x->power += on ? 1 : -1;
 556
 557         WARN_ON(ov965x->power < 0);
 558         mutex_unlock(&ov965x->lock);
 559         return ret;
 560 }
 561
 562 /*
 563  * V4L2 controls
 564  */
 565
 566 static void ov965x_update_exposure_ctrl(struct ov965x *ov965x)
 567 {
 568         struct v4l2_ctrl *ctrl = ov965x->ctrls.exposure;
 569         unsigned long fint, trow;
 570         int min, max, def;
 571         u8 clkrc;
 572
 573         mutex_lock(&ov965x->lock);
 574         if (WARN_ON(!ctrl || !ov965x->frame_size)) {
 575                 mutex_unlock(&ov965x->lock);
 576                 return;
 577         }
 578         clkrc = DEF_CLKRC + ov965x->fiv->clkrc_div;
 579         /* Calculate internal clock frequency */
 580         fint = ov965x->mclk_frequency * ((clkrc >> 7) + 1) /
 581                                 ((2 * ((clkrc & 0x3f) + 1)));
 582         /* and the row interval (in us). */
 583         trow = (2 * 1520 * 1000000UL) / fint;
 584         max = ov965x->frame_size->max_exp_lines * trow;
 585         ov965x->exp_row_interval = trow;
 586         mutex_unlock(&ov965x->lock);
 587
 588         v4l2_dbg(1, debug, &ov965x->sd, "clkrc: %#x, fi: %lu, tr: %lu, %d\n",
 589                  clkrc, fint, trow, max);
 590
 591         /* Update exposure time range to match current frame format. */
 592         min = (trow + 100) / 100;
 593         max = (max - 100) / 100;
 594         def = min + (max - min) / 2;
 595
 596         if (v4l2_ctrl_modify_range(ctrl, min, max, 1, def))
 597                 v4l2_err(&ov965x->sd, "Exposure ctrl range update failed\n");
 598 }
 599
 600 static int ov965x_set_banding_filter(struct ov965x *ov965x, int value)
 601 {
 602         unsigned long mbd, light_freq;
 603         int ret;
 604         u8 reg;
 605
 606         ret = ov965x_read(ov965x->client, REG_COM8, &reg);
 607         if (!ret) {
 608                 if (value == V4L2_CID_POWER_LINE_FREQUENCY_DISABLED)
 609                         reg &= ~COM8_BFILT;
 610                 else
 611                         reg |= COM8_BFILT;
 612                 ret = ov965x_write(ov965x->client, REG_COM8, reg);
 613         }
 614         if (value == V4L2_CID_POWER_LINE_FREQUENCY_DISABLED)
 615                 return 0;
 616         if (WARN_ON(!ov965x->fiv))
 617                 return -EINVAL;
 618         /* Set minimal exposure time for 50/60 HZ lighting */
 619         if (value == V4L2_CID_POWER_LINE_FREQUENCY_50HZ)
 620                 light_freq = 50;
 621         else
 622                 light_freq = 60;
 623         mbd = (1000UL * ov965x->fiv->interval.denominator *
 624                ov965x->frame_size->max_exp_lines) /
 625                ov965x->fiv->interval.numerator;
 626         mbd = ((mbd / (light_freq * 2)) + 500) / 1000UL;
 627
 628         return ov965x_write(ov965x->client, REG_MBD, mbd);
 629 }
 630
 631 static int ov965x_set_white_balance(struct ov965x *ov965x, int awb)
 632 {
 633         int ret;
 634         u8 reg;
 635
 636         ret = ov965x_read(ov965x->client, REG_COM8, &reg);
 637         if (!ret) {
 638                 reg = awb ? reg | REG_COM8 : reg & ~REG_COM8;
 639                 ret = ov965x_write(ov965x->client, REG_COM8, reg);
 640         }
 641         if (!ret && !awb) {
 642                 ret = ov965x_write(ov965x->client, REG_BLUE,
 643                                    ov965x->ctrls.blue_balance->val);
 644                 if (ret < 0)
 645                         return ret;
 646                 ret = ov965x_write(ov965x->client, REG_RED,
 647                                    ov965x->ctrls.red_balance->val);
 648         }
 649         return ret;
 650 }
 651
 652 #define NUM_BR_LEVELS   7
 653 #define NUM_BR_REGS     3
 654
 655 static int ov965x_set_brightness(struct ov965x *ov965x, int val)
 656 {
 657         static const u8 regs[NUM_BR_LEVELS + 1][NUM_BR_REGS] = {
 658                 { REG_AEW, REG_AEB, REG_VPT },
 659                 { 0x1c, 0x12, 0x50 }, /* -3 */
 660                 { 0x3d, 0x30, 0x71 }, /* -2 */
 661                 { 0x50, 0x44, 0x92 }, /* -1 */
 662                 { 0x70, 0x64, 0xc3 }, /*  0 */
 663                 { 0x90, 0x84, 0xd4 }, /* +1 */
 664                 { 0xc4, 0xbf, 0xf9 }, /* +2 */
 665                 { 0xd8, 0xd0, 0xfa }, /* +3 */
 666         };
 667         int i, ret = 0;
 668
 669         val += (NUM_BR_LEVELS / 2 + 1);
 670         if (val > NUM_BR_LEVELS)
 671                 return -EINVAL;
 672
 673         for (i = 0; i < NUM_BR_REGS && !ret; i++)
 674                 ret = ov965x_write(ov965x->client, regs[0][i],
 675                                    regs[val][i]);
 676         return ret;
 677 }
 678
 679 static int ov965x_set_gain(struct ov965x *ov965x, int auto_gain)
 680 {
 681         struct i2c_client *client = ov965x->client;
 682         struct ov965x_ctrls *ctrls = &ov965x->ctrls;
 683         int ret = 0;
 684         u8 reg;
 685         /*
 686          * For manual mode we need to disable AGC first, so
 687          * gain value in REG_VREF, REG_GAIN is not overwritten.
 688          */
 689         if (ctrls->auto_gain->is_new) {
 690                 ret = ov965x_read(client, REG_COM8, &reg);
 691                 if (ret < 0)
 692                         return ret;
 693                 if (ctrls->auto_gain->val)
 694                         reg |= COM8_AGC;
 695                 else
 696                         reg &= ~COM8_AGC;
 697                 ret = ov965x_write(client, REG_COM8, reg);
 698                 if (ret < 0)
 699                         return ret;
 700         }
 701
 702         if (ctrls->gain->is_new && !auto_gain) {
 703                 unsigned int gain = ctrls->gain->val;
 704                 unsigned int rgain;
 705                 int m;
 706                 /*
 707                  * Convert gain control value to the sensor's gain
 708                  * registers (VREF[7:6], GAIN[7:0]) format.
 709                  */
 710                 for (m = 6; m >= 0; m--)
 711                         if (gain >= (1 << m) * 16)
 712                                 break;
 713
 714                 /* Sanity check: don't adjust the gain with a negative value */
 715                 if (m < 0)
 716                         return -EINVAL;
 717
 718                 rgain = (gain - ((1 << m) * 16)) / (1 << m);
 719                 rgain |= (((1 << m) - 1) << 4);
 720
 721                 ret = ov965x_write(client, REG_GAIN, rgain & 0xff);
 722                 if (ret < 0)
 723                         return ret;
 724                 ret = ov965x_read(client, REG_VREF, &reg);
 725                 if (ret < 0)
 726                         return ret;
 727                 reg &= ~VREF_GAIN_MASK;
 728                 reg |= (((rgain >> 8) & 0x3) << 6);
 729                 ret = ov965x_write(client, REG_VREF, reg);
 730                 if (ret < 0)
 731                         return ret;
 732                 /* Return updated control's value to userspace */
 733                 ctrls->gain->val = (1 << m) * (16 + (rgain & 0xf));
 734         }
 735
 736         return ret;
 737 }
 738
 739 static int ov965x_set_sharpness(struct ov965x *ov965x, unsigned int value)
 740 {
 741         u8 com14, edge;
 742         int ret;
 743
 744         ret = ov965x_read(ov965x->client, REG_COM14, &com14);
 745         if (ret < 0)
 746                 return ret;
 747         ret = ov965x_read(ov965x->client, REG_EDGE, &edge);
 748         if (ret < 0)
 749                 return ret;
 750         com14 = value ? com14 | COM14_EDGE_EN : com14 & ~COM14_EDGE_EN;
 751         value--;
 752         if (value > 0x0f) {
 753                 com14 |= COM14_EEF_X2;
 754                 value >>= 1;
 755         } else {
 756                 com14 &= ~COM14_EEF_X2;
 757         }
 758         ret = ov965x_write(ov965x->client, REG_COM14, com14);
 759         if (ret < 0)
 760                 return ret;
 761
 762         edge &= ~EDGE_FACTOR_MASK;
 763         edge |= ((u8)value & 0x0f);
 764
 765         return ov965x_write(ov965x->client, REG_EDGE, edge);
 766 }
 767
 768 static int ov965x_set_exposure(struct ov965x *ov965x, int exp)
 769 {
 770         struct i2c_client *client = ov965x->client;
 771         struct ov965x_ctrls *ctrls = &ov965x->ctrls;
 772         bool auto_exposure = (exp == V4L2_EXPOSURE_AUTO);
 773         int ret;
 774         u8 reg;
 775
 776         if (ctrls->auto_exp->is_new) {
 777                 ret = ov965x_read(client, REG_COM8, &reg);
 778                 if (ret < 0)
 779                         return ret;
 780                 if (auto_exposure)
 781                         reg |= (COM8_AEC | COM8_AGC);
 782                 else
 783                         reg &= ~(COM8_AEC | COM8_AGC);
 784                 ret = ov965x_write(client, REG_COM8, reg);
 785                 if (ret < 0)
 786                         return ret;
 787         }
 788
 789         if (!auto_exposure && ctrls->exposure->is_new) {
 790                 unsigned int exposure = (ctrls->exposure->val * 100)
 791                                          / ov965x->exp_row_interval;
 792                 /*
 793                  * Manual exposure value
 794                  * [b15:b0] - AECHM (b15:b10), AECH (b9:b2), COM1 (b1:b0)
 795                  */
 796                 ret = ov965x_write(client, REG_COM1, exposure & 0x3);
 797                 if (!ret)
 798                         ret = ov965x_write(client, REG_AECH,
 799                                            (exposure >> 2) & 0xff);
 800                 if (!ret)
 801                         ret = ov965x_write(client, REG_AECHM,
 802                                            (exposure >> 10) & 0x3f);
 803                 /* Update the value to minimize rounding errors */
 804                 ctrls->exposure->val = ((exposure * ov965x->exp_row_interval)
 805                                                         + 50) / 100;
 806                 if (ret < 0)
 807                         return ret;
 808         }
 809
 810         v4l2_ctrl_activate(ov965x->ctrls.brightness, !exp);
 811         return 0;
 812 }
 813
 814 static int ov965x_set_flip(struct ov965x *ov965x)
 815 {
 816         u8 mvfp = 0;
 817
 818         if (ov965x->ctrls.hflip->val)
 819                 mvfp |= MVFP_MIRROR;
 820
 821         if (ov965x->ctrls.vflip->val)
 822                 mvfp |= MVFP_FLIP;
 823
 824         return ov965x_write(ov965x->client, REG_MVFP, mvfp);
 825 }
 826
 827 #define NUM_SAT_LEVELS  5
 828 #define NUM_SAT_REGS    6
 829
 830 static int ov965x_set_saturation(struct ov965x *ov965x, int val)
 831 {
 832         static const u8 regs[NUM_SAT_LEVELS][NUM_SAT_REGS] = {
 833                 /* MTX(1)...MTX(6) */
 834                 { 0x1d, 0x1f, 0x02, 0x09, 0x13, 0x1c }, /* -2 */
 835                 { 0x2e, 0x31, 0x02, 0x0e, 0x1e, 0x2d }, /* -1 */
 836                 { 0x3a, 0x3d, 0x03, 0x12, 0x26, 0x38 }, /*  0 */
 837                 { 0x46, 0x49, 0x04, 0x16, 0x2e, 0x43 }, /* +1 */
 838                 { 0x57, 0x5c, 0x05, 0x1b, 0x39, 0x54 }, /* +2 */
 839         };
 840         u8 addr = REG_MTX(1);
 841         int i, ret = 0;
 842
 843         val += (NUM_SAT_LEVELS / 2);
 844         if (val >= NUM_SAT_LEVELS)
 845                 return -EINVAL;
 846
 847         for (i = 0; i < NUM_SAT_REGS && !ret; i++)
 848                 ret = ov965x_write(ov965x->client, addr + i, regs[val][i]);
 849
 850         return ret;
 851 }
 852
 853 static int ov965x_set_test_pattern(struct ov965x *ov965x, int value)
 854 {
 855         int ret;
 856         u8 reg;
 857
 858         ret = ov965x_read(ov965x->client, REG_COM23, &reg);
 859         if (ret < 0)
 860                 return ret;
 861         reg = value ? reg | COM23_TEST_MODE : reg & ~COM23_TEST_MODE;
 862         return ov965x_write(ov965x->client, REG_COM23, reg);
 863 }
 864
 865 static int __g_volatile_ctrl(struct ov965x *ov965x, struct v4l2_ctrl *ctrl)
 866 {
 867         struct i2c_client *client = ov965x->client;
 868         unsigned int exposure, gain, m;
 869         u8 reg0, reg1, reg2;
 870         int ret;
 871
 872         if (!ov965x->power)
 873                 return 0;
 874
 875         switch (ctrl->id) {
 876         case V4L2_CID_AUTOGAIN:
 877                 if (!ctrl->val)
 878                         return 0;
 879                 ret = ov965x_read(client, REG_GAIN, &reg0);
 880                 if (ret < 0)
 881                         return ret;
 882                 ret = ov965x_read(client, REG_VREF, &reg1);
 883                 if (ret < 0)
 884                         return ret;
 885                 gain = ((reg1 >> 6) << 8) | reg0;
 886                 m = 0x01 << fls(gain >> 4);
 887                 ov965x->ctrls.gain->val = m * (16 + (gain & 0xf));
 888                 break;
 889
 890         case V4L2_CID_EXPOSURE_AUTO:
 891                 if (ctrl->val == V4L2_EXPOSURE_MANUAL)
 892                         return 0;
 893                 ret = ov965x_read(client, REG_COM1, &reg0);
 894                 if (!ret)
 895                         ret = ov965x_read(client, REG_AECH, &reg1);
 896                 if (!ret)
 897                         ret = ov965x_read(client, REG_AECHM, &reg2);
 898                 if (ret < 0)
 899                         return ret;
 900                 exposure = ((reg2 & 0x3f) << 10) | (reg1 << 2) |
 901                                                 (reg0 & 0x3);
 902                 ov965x->ctrls.exposure->val = ((exposure *
 903                                 ov965x->exp_row_interval) + 50) / 100;
 904                 break;
 905         }
 906
 907         return 0;
 908 }
 909
 910 static int ov965x_g_volatile_ctrl(struct v4l2_ctrl *ctrl)
 911 {
 912         struct v4l2_subdev *sd = ctrl_to_sd(ctrl);
 913         struct ov965x *ov965x = to_ov965x(sd);
 914         int ret;
 915
 916         v4l2_dbg(1, debug, sd, "g_ctrl: %s\n", ctrl->name);
 917
 918         mutex_lock(&ov965x->lock);
 919         ret = __g_volatile_ctrl(ov965x, ctrl);
 920         mutex_unlock(&ov965x->lock);
 921         return ret;
 922 }
 923
 924 static int ov965x_s_ctrl(struct v4l2_ctrl *ctrl)
 925 {
 926         struct v4l2_subdev *sd = ctrl_to_sd(ctrl);
 927         struct ov965x *ov965x = to_ov965x(sd);
 928         int ret = -EINVAL;
 929
 930         v4l2_dbg(1, debug, sd, "s_ctrl: %s, value: %d. power: %d\n",
 931                  ctrl->name, ctrl->val, ov965x->power);
 932
 933         mutex_lock(&ov965x->lock);
 934         /*
 935          * If the device is not powered up now postpone applying control's
 936          * value to the hardware, until it is ready to accept commands.
 937          */
 938         if (ov965x->power == 0) {
 939                 mutex_unlock(&ov965x->lock);
 940                 return 0;
 941         }
 942
 943         switch (ctrl->id) {
 944         case V4L2_CID_AUTO_WHITE_BALANCE:
 945                 ret = ov965x_set_white_balance(ov965x, ctrl->val);
 946                 break;
 947
 948         case V4L2_CID_BRIGHTNESS:
 949                 ret = ov965x_set_brightness(ov965x, ctrl->val);
 950                 break;
 951
 952         case V4L2_CID_EXPOSURE_AUTO:
 953                 ret = ov965x_set_exposure(ov965x, ctrl->val);
 954                 break;
 955
 956         case V4L2_CID_AUTOGAIN:
 957                 ret = ov965x_set_gain(ov965x, ctrl->val);
 958                 break;
 959
 960         case V4L2_CID_HFLIP:
 961                 ret = ov965x_set_flip(ov965x);
 962                 break;
 963
 964         case V4L2_CID_POWER_LINE_FREQUENCY:
 965                 ret = ov965x_set_banding_filter(ov965x, ctrl->val);
 966                 break;
 967
 968         case V4L2_CID_SATURATION:
 969                 ret = ov965x_set_saturation(ov965x, ctrl->val);
 970                 break;
 971
 972         case V4L2_CID_SHARPNESS:
 973                 ret = ov965x_set_sharpness(ov965x, ctrl->val);
 974                 break;
 975
 976         case V4L2_CID_TEST_PATTERN:
 977                 ret = ov965x_set_test_pattern(ov965x, ctrl->val);
 978                 break;
 979         }
 980
 981         mutex_unlock(&ov965x->lock);
 982         return ret;
 983 }
 984
 985 static const struct v4l2_ctrl_ops ov965x_ctrl_ops = {
 986         .g_volatile_ctrl = ov965x_g_volatile_ctrl,
 987         .s_ctrl = ov965x_s_ctrl,
 988 };
 989
 990 static const char * const test_pattern_menu[] = {
 991         "Disabled",
 992         "Color bars",
 993         NULL
 994 };
 995
 996 static int ov965x_initialize_controls(struct ov965x *ov965x)
 997 {
 998         const struct v4l2_ctrl_ops *ops = &ov965x_ctrl_ops;
 999         struct ov965x_ctrls *ctrls = &ov965x->ctrls;
1000         struct v4l2_ctrl_handler *hdl = &ctrls->handler;
1001         int ret;
1002
1003         ret = v4l2_ctrl_handler_init(hdl, 16);
1004         if (ret < 0)
1005                 return ret;
1006
1007         /* Auto/manual white balance */
1008         ctrls->auto_wb = v4l2_ctrl_new_std(hdl, ops,
1009                                            V4L2_CID_AUTO_WHITE_BALANCE,
1010                                            0, 1, 1, 1);
1011         ctrls->blue_balance = v4l2_ctrl_new_std(hdl, ops, V4L2_CID_BLUE_BALANCE,
1012                                                 0, 0xff, 1, 0x80);
1013         ctrls->red_balance = v4l2_ctrl_new_std(hdl, ops, V4L2_CID_RED_BALANCE,
1014                                                0, 0xff, 1, 0x80);
1015         /* Auto/manual exposure */
1016         ctrls->auto_exp =
1017                 v4l2_ctrl_new_std_menu(hdl, ops,
1018                                        V4L2_CID_EXPOSURE_AUTO,
1019                                        V4L2_EXPOSURE_MANUAL, 0,
1020                                        V4L2_EXPOSURE_AUTO);
1021         /* Exposure time, in 100 us units. min/max is updated dynamically. */
1022         ctrls->exposure = v4l2_ctrl_new_std(hdl, ops,
1023                                             V4L2_CID_EXPOSURE_ABSOLUTE,
1024                                             2, 1500, 1, 500);
1025         /* Auto/manual gain */
1026         ctrls->auto_gain = v4l2_ctrl_new_std(hdl, ops, V4L2_CID_AUTOGAIN,
1027                                              0, 1, 1, 1);
1028         ctrls->gain = v4l2_ctrl_new_std(hdl, ops, V4L2_CID_GAIN,
1029                                         16, 64 * (16 + 15), 1, 64 * 16);
1030
1031         ctrls->saturation = v4l2_ctrl_new_std(hdl, ops, V4L2_CID_SATURATION,
1032                                               -2, 2, 1, 0);
1033         ctrls->brightness = v4l2_ctrl_new_std(hdl, ops, V4L2_CID_BRIGHTNESS,
1034                                               -3, 3, 1, 0);
1035         ctrls->sharpness = v4l2_ctrl_new_std(hdl, ops, V4L2_CID_SHARPNESS,
1036                                              0, 32, 1, 6);
1037
1038         ctrls->hflip = v4l2_ctrl_new_std(hdl, ops, V4L2_CID_HFLIP, 0, 1, 1, 0);
1039         ctrls->vflip = v4l2_ctrl_new_std(hdl, ops, V4L2_CID_VFLIP, 0, 1, 1, 0);
1040
1041         ctrls->light_freq =
1042                 v4l2_ctrl_new_std_menu(hdl, ops,
1043                                        V4L2_CID_POWER_LINE_FREQUENCY,
1044                                        V4L2_CID_POWER_LINE_FREQUENCY_60HZ, ~0x7,
1045                                        V4L2_CID_POWER_LINE_FREQUENCY_50HZ);
1046
1047         v4l2_ctrl_new_std_menu_items(hdl, ops, V4L2_CID_TEST_PATTERN,
1048                                      ARRAY_SIZE(test_pattern_menu) - 1, 0, 0,
1049                                      test_pattern_menu);
1050         if (hdl->error) {
1051                 ret = hdl->error;
1052                 v4l2_ctrl_handler_free(hdl);
1053                 return ret;
1054         }
1055
1056         ctrls->gain->flags |= V4L2_CTRL_FLAG_VOLATILE;
1057         ctrls->exposure->flags |= V4L2_CTRL_FLAG_VOLATILE;
1058
1059         v4l2_ctrl_auto_cluster(3, &ctrls->auto_wb, 0, false);
1060         v4l2_ctrl_auto_cluster(2, &ctrls->auto_gain, 0, true);
1061         v4l2_ctrl_auto_cluster(2, &ctrls->auto_exp, 1, true);
1062         v4l2_ctrl_cluster(2, &ctrls->hflip);
1063
1064         ov965x->sd.ctrl_handler = hdl;
1065         return 0;
1066 }
1067
1068 /*
1069  * V4L2 subdev video and pad level operations
1070  */
1071 static void ov965x_get_default_format(struct v4l2_mbus_framefmt *mf)
1072 {
1073         mf->width = ov965x_framesizes[0].width;
1074         mf->height = ov965x_framesizes[0].height;
1075         mf->colorspace = ov965x_formats[0].colorspace;
1076         mf->code = ov965x_formats[0].code;
1077         mf->field = V4L2_FIELD_NONE;
1078 }
1079
1080 static int ov965x_enum_mbus_code(struct v4l2_subdev *sd,
1081                                  struct v4l2_subdev_pad_config *cfg,
1082                                  struct v4l2_subdev_mbus_code_enum *code)
1083 {
1084         if (code->index >= ARRAY_SIZE(ov965x_formats))
1085                 return -EINVAL;
1086
1087         code->code = ov965x_formats[code->index].code;
1088         return 0;
1089 }
1090
1091 static int ov965x_enum_frame_sizes(struct v4l2_subdev *sd,
1092                                    struct v4l2_subdev_pad_config *cfg,
1093                                    struct v4l2_subdev_frame_size_enum *fse)
1094 {
1095         int i = ARRAY_SIZE(ov965x_formats);
1096
1097         if (fse->index >= ARRAY_SIZE(ov965x_framesizes))
1098                 return -EINVAL;
1099
1100         while (--i)
1101                 if (fse->code == ov965x_formats[i].code)
1102                         break;
1103
1104         fse->code = ov965x_formats[i].code;
1105
1106         fse->min_width  = ov965x_framesizes[fse->index].width;
1107         fse->max_width  = fse->min_width;
1108         fse->max_height = ov965x_framesizes[fse->index].height;
1109         fse->min_height = fse->max_height;
1110
1111         return 0;
1112 }
1113
1114 static int ov965x_g_frame_interval(struct v4l2_subdev *sd,
1115                                    struct v4l2_subdev_frame_interval *fi)
1116 {
1117         struct ov965x *ov965x = to_ov965x(sd);
1118
1119         mutex_lock(&ov965x->lock);
1120         fi->interval = ov965x->fiv->interval;
1121         mutex_unlock(&ov965x->lock);
1122
1123         return 0;
1124 }
1125
1126 static int __ov965x_set_frame_interval(struct ov965x *ov965x,
1127                                        struct v4l2_subdev_frame_interval *fi)
1128 {
1129         struct v4l2_mbus_framefmt *mbus_fmt = &ov965x->format;
1130         const struct ov965x_interval *fiv = &ov965x_intervals[0];
1131         u64 req_int, err, min_err = ~0ULL;
1132         unsigned int i;
1133
1134         if (fi->interval.denominator == 0)
1135                 return -EINVAL;
1136
1137         req_int = (u64)(fi->interval.numerator * 10000) /
1138                 fi->interval.denominator;
1139
1140         for (i = 0; i < ARRAY_SIZE(ov965x_intervals); i++) {
1141                 const struct ov965x_interval *iv = &ov965x_intervals[i];
1142
1143                 if (mbus_fmt->width != iv->size.width ||
1144                     mbus_fmt->height != iv->size.height)
1145                         continue;
1146                 err = abs((u64)(iv->interval.numerator * 10000) /
1147                             iv->interval.denominator - req_int);
1148                 if (err < min_err) {
1149                         fiv = iv;
1150                         min_err = err;
1151                 }
1152         }
1153         ov965x->fiv = fiv;
1154
1155         v4l2_dbg(1, debug, &ov965x->sd, "Changed frame interval to %u us\n",
1156                  fiv->interval.numerator * 1000000 / fiv->interval.denominator);
1157
1158         return 0;
1159 }
1160
1161 static int ov965x_s_frame_interval(struct v4l2_subdev *sd,
1162                                    struct v4l2_subdev_frame_interval *fi)
1163 {
1164         struct ov965x *ov965x = to_ov965x(sd);
1165         int ret;
1166
1167         v4l2_dbg(1, debug, sd, "Setting %d/%d frame interval\n",
1168                  fi->interval.numerator, fi->interval.denominator);
1169
1170         mutex_lock(&ov965x->lock);
1171         ret = __ov965x_set_frame_interval(ov965x, fi);
1172         ov965x->apply_frame_fmt = 1;
1173         mutex_unlock(&ov965x->lock);
1174         return ret;
1175 }
1176
1177 static int ov965x_get_fmt(struct v4l2_subdev *sd,
1178                           struct v4l2_subdev_pad_config *cfg,
1179                           struct v4l2_subdev_format *fmt)
1180 {
1181         struct ov965x *ov965x = to_ov965x(sd);
1182         struct v4l2_mbus_framefmt *mf;
1183
1184         if (fmt->which == V4L2_SUBDEV_FORMAT_TRY) {
1185                 mf = v4l2_subdev_get_try_format(sd, cfg, 0);
1186                 fmt->format = *mf;
1187                 return 0;
1188         }
1189
1190         mutex_lock(&ov965x->lock);
1191         fmt->format = ov965x->format;
1192         mutex_unlock(&ov965x->lock);
1193
1194         return 0;
1195 }
1196
1197 static void __ov965x_try_frame_size(struct v4l2_mbus_framefmt *mf,
1198                                     const struct ov965x_framesize **size)
1199 {
1200         const struct ov965x_framesize *fsize = &ov965x_framesizes[0],
1201                 *match = NULL;
1202         int i = ARRAY_SIZE(ov965x_framesizes);
1203         unsigned int min_err = UINT_MAX;
1204
1205         while (i--) {
1206                 int err = abs(fsize->width - mf->width)
1207                                 + abs(fsize->height - mf->height);
1208                 if (err < min_err) {
1209                         min_err = err;
1210                         match = fsize;
1211                 }
1212                 fsize++;
1213         }
1214         if (!match)
1215                 match = &ov965x_framesizes[0];
1216         mf->width  = match->width;
1217         mf->height = match->height;
1218         if (size)
1219                 *size = match;
1220 }
1221
1222 static int ov965x_set_fmt(struct v4l2_subdev *sd,
1223                           struct v4l2_subdev_pad_config *cfg,
1224                           struct v4l2_subdev_format *fmt)
1225 {
1226         unsigned int index = ARRAY_SIZE(ov965x_formats);
1227         struct v4l2_mbus_framefmt *mf = &fmt->format;
1228         struct ov965x *ov965x = to_ov965x(sd);
1229         const struct ov965x_framesize *size = NULL;
1230         int ret = 0;
1231
1232         __ov965x_try_frame_size(mf, &size);
1233
1234         while (--index)
1235                 if (ov965x_formats[index].code == mf->code)
1236                         break;
1237
1238         mf->colorspace  = V4L2_COLORSPACE_JPEG;
1239         mf->code        = ov965x_formats[index].code;
1240         mf->field       = V4L2_FIELD_NONE;
1241
1242         mutex_lock(&ov965x->lock);
1243
1244         if (fmt->which == V4L2_SUBDEV_FORMAT_TRY) {
1245                 if (cfg) {
1246                         mf = v4l2_subdev_get_try_format(sd, cfg, fmt->pad);
1247                         *mf = fmt->format;
1248                 }
1249         } else {
1250                 if (ov965x->streaming) {
1251                         ret = -EBUSY;
1252                 } else {
1253                         ov965x->frame_size = size;
1254                         ov965x->format = fmt->format;
1255                         ov965x->tslb_reg = ov965x_formats[index].tslb_reg;
1256                         ov965x->apply_frame_fmt = 1;
1257                 }
1258         }
1259
1260         if (!ret && fmt->which == V4L2_SUBDEV_FORMAT_ACTIVE) {
1261                 struct v4l2_subdev_frame_interval fiv = {
1262                         .interval = { 0, 1 }
1263                 };
1264                 /* Reset to minimum possible frame interval */
1265                 __ov965x_set_frame_interval(ov965x, &fiv);
1266         }
1267         mutex_unlock(&ov965x->lock);
1268
1269         if (!ret)
1270                 ov965x_update_exposure_ctrl(ov965x);
1271
1272         return ret;
1273 }
1274
1275 static int ov965x_set_frame_size(struct ov965x *ov965x)
1276 {
1277         int i, ret = 0;
1278
1279         for (i = 0; ret == 0 && i < NUM_FMT_REGS; i++)
1280                 ret = ov965x_write(ov965x->client, frame_size_reg_addr[i],
1281                                    ov965x->frame_size->regs[i]);
1282         return ret;
1283 }
1284
1285 static int __ov965x_set_params(struct ov965x *ov965x)
1286 {
1287         struct i2c_client *client = ov965x->client;
1288         struct ov965x_ctrls *ctrls = &ov965x->ctrls;
1289         int ret = 0;
1290         u8 reg;
1291
1292         if (ov965x->apply_frame_fmt) {
1293                 reg = DEF_CLKRC + ov965x->fiv->clkrc_div;
1294                 ret = ov965x_write(client, REG_CLKRC, reg);
1295                 if (ret < 0)
1296                         return ret;
1297                 ret = ov965x_set_frame_size(ov965x);
1298                 if (ret < 0)
1299                         return ret;
1300                 ret = ov965x_read(client, REG_TSLB, &reg);
1301                 if (ret < 0)
1302                         return ret;
1303                 reg &= ~TSLB_YUYV_MASK;
1304                 reg |= ov965x->tslb_reg;
1305                 ret = ov965x_write(client, REG_TSLB, reg);
1306                 if (ret < 0)
1307                         return ret;
1308         }
1309         ret = ov965x_set_default_gamma_curve(ov965x);
1310         if (ret < 0)
1311                 return ret;
1312         ret = ov965x_set_color_matrix(ov965x);
1313         if (ret < 0)
1314                 return ret;
1315         /*
1316          * Select manual banding filter, the filter will
1317          * be enabled further if required.
1318          */
1319         ret = ov965x_read(client, REG_COM11, &reg);
1320         if (!ret)
1321                 reg |= COM11_BANDING;
1322         ret = ov965x_write(client, REG_COM11, reg);
1323         if (ret < 0)
1324                 return ret;
1325         /*
1326          * Banding filter (REG_MBD value) needs to match selected
1327          * resolution and frame rate, so it's always updated here.
1328          */
1329         return ov965x_set_banding_filter(ov965x, ctrls->light_freq->val);
1330 }
1331
1332 static int ov965x_s_stream(struct v4l2_subdev *sd, int on)
1333 {
1334         struct i2c_client *client = v4l2_get_subdevdata(sd);
1335         struct ov965x *ov965x = to_ov965x(sd);
1336         struct ov965x_ctrls *ctrls = &ov965x->ctrls;
1337         int ret = 0;
1338
1339         v4l2_dbg(1, debug, client, "%s: on: %d\n", __func__, on);
1340
1341         mutex_lock(&ov965x->lock);
1342         if (ov965x->streaming == !on) {
1343                 if (on)
1344                         ret = __ov965x_set_params(ov965x);
1345
1346                 if (!ret && ctrls->update) {
1347                         /*
1348                          * ov965x_s_ctrl callback takes the mutex
1349                          * so it needs to be released here.
1350                          */
1351                         mutex_unlock(&ov965x->lock);
1352                         ret = v4l2_ctrl_handler_setup(&ctrls->handler);
1353
1354                         mutex_lock(&ov965x->lock);
1355                         if (!ret)
1356                                 ctrls->update = 0;
1357                 }
1358                 if (!ret)
1359                         ret = ov965x_write(client, REG_COM2,
1360                                            on ? 0x01 : 0x11);
1361         }
1362         if (!ret)
1363                 ov965x->streaming += on ? 1 : -1;
1364
1365         WARN_ON(ov965x->streaming < 0);
1366         mutex_unlock(&ov965x->lock);
1367
1368         return ret;
1369 }
1370
1371 /*
1372  * V4L2 subdev internal operations
1373  */
1374 static int ov965x_open(struct v4l2_subdev *sd, struct v4l2_subdev_fh *fh)
1375 {
1376         struct v4l2_mbus_framefmt *mf =
1377                 v4l2_subdev_get_try_format(sd, fh->pad, 0);
1378
1379         ov965x_get_default_format(mf);
1380         return 0;
1381 }
1382
1383 static const struct v4l2_subdev_pad_ops ov965x_pad_ops = {
1384         .enum_mbus_code = ov965x_enum_mbus_code,
1385         .enum_frame_size = ov965x_enum_frame_sizes,
1386         .get_fmt = ov965x_get_fmt,
1387         .set_fmt = ov965x_set_fmt,
1388 };
1389
1390 static const struct v4l2_subdev_video_ops ov965x_video_ops = {
1391         .s_stream = ov965x_s_stream,
1392         .g_frame_interval = ov965x_g_frame_interval,
1393         .s_frame_interval = ov965x_s_frame_interval,
1394
1395 };
1396
1397 static const struct v4l2_subdev_internal_ops ov965x_sd_internal_ops = {
1398         .open = ov965x_open,
1399 };
1400
1401 static const struct v4l2_subdev_core_ops ov965x_core_ops = {
1402         .s_power = ov965x_s_power,
1403         .log_status = v4l2_ctrl_subdev_log_status,
1404         .subscribe_event = v4l2_ctrl_subdev_subscribe_event,
1405         .unsubscribe_event = v4l2_event_subdev_unsubscribe,
1406 };
1407
1408 static const struct v4l2_subdev_ops ov965x_subdev_ops = {
1409         .core = &ov965x_core_ops,
1410         .pad = &ov965x_pad_ops,
1411         .video = &ov965x_video_ops,
1412 };
1413
1414 /*
1415  * Reset and power down GPIOs configuration
1416  */
1417 static int ov965x_configure_gpios(struct ov965x *ov965x,
1418                                   const struct ov9650_platform_data *pdata)
1419 {
1420         int ret, i;
1421
1422         ov965x->gpios[GPIO_PWDN] = pdata->gpio_pwdn;
1423         ov965x->gpios[GPIO_RST]  = pdata->gpio_reset;
1424
1425         for (i = 0; i < ARRAY_SIZE(ov965x->gpios); i++) {
1426                 int gpio = ov965x->gpios[i];
1427
1428                 if (!gpio_is_valid(gpio))
1429                         continue;
1430                 ret = devm_gpio_request_one(&ov965x->client->dev, gpio,
1431                                             GPIOF_OUT_INIT_HIGH, "OV965X");
1432                 if (ret < 0)
1433                         return ret;
1434                 v4l2_dbg(1, debug, &ov965x->sd, "set gpio %d to 1\n", gpio);
1435
1436                 gpio_set_value(gpio, 1);
1437                 gpio_export(gpio, 0);
1438                 ov965x->gpios[i] = gpio;
1439         }
1440
1441         return 0;
1442 }
1443
1444 static int ov965x_detect_sensor(struct v4l2_subdev *sd)
1445 {
1446         struct i2c_client *client = v4l2_get_subdevdata(sd);
1447         struct ov965x *ov965x = to_ov965x(sd);
1448         u8 pid, ver;
1449         int ret;
1450
1451         mutex_lock(&ov965x->lock);
1452         __ov965x_set_power(ov965x, 1);
1453         msleep(25);
1454
1455         /* Check sensor revision */
1456         ret = ov965x_read(client, REG_PID, &pid);
1457         if (!ret)
1458                 ret = ov965x_read(client, REG_VER, &ver);
1459
1460         __ov965x_set_power(ov965x, 0);
1461
1462         if (!ret) {
1463                 ov965x->id = OV965X_ID(pid, ver);
1464                 if (ov965x->id == OV9650_ID || ov965x->id == OV9652_ID) {
1465                         v4l2_info(sd, "Found OV%04X sensor\n", ov965x->id);
1466                 } else {
1467                         v4l2_err(sd, "Sensor detection failed (%04X, %d)\n",
1468                                  ov965x->id, ret);
1469                         ret = -ENODEV;
1470                 }
1471         }
1472         mutex_unlock(&ov965x->lock);
1473
1474         return ret;
1475 }
1476
1477 static int ov965x_probe(struct i2c_client *client,
1478                         const struct i2c_device_id *id)
1479 {
1480         const struct ov9650_platform_data *pdata = client->dev.platform_data;
1481         struct v4l2_subdev *sd;
1482         struct ov965x *ov965x;
1483         int ret;
1484
1485         if (!pdata) {
1486                 dev_err(&client->dev, "platform data not specified\n");
1487                 return -EINVAL;
1488         }
1489
1490         if (pdata->mclk_frequency == 0) {
1491                 dev_err(&client->dev, "MCLK frequency not specified\n");
1492                 return -EINVAL;
1493         }
1494
1495         ov965x = devm_kzalloc(&client->dev, sizeof(*ov965x), GFP_KERNEL);
1496         if (!ov965x)
1497                 return -ENOMEM;
1498
1499         mutex_init(&ov965x->lock);
1500         ov965x->client = client;
1501         ov965x->mclk_frequency = pdata->mclk_frequency;
1502
1503         sd = &ov965x->sd;
1504         v4l2_i2c_subdev_init(sd, client, &ov965x_subdev_ops);
1505         strlcpy(sd->name, DRIVER_NAME, sizeof(sd->name));
1506
1507         sd->internal_ops = &ov965x_sd_internal_ops;
1508         sd->flags |= V4L2_SUBDEV_FL_HAS_DEVNODE |
1509                      V4L2_SUBDEV_FL_HAS_EVENTS;
1510
1511         ret = ov965x_configure_gpios(ov965x, pdata);
1512         if (ret < 0)
1513                 goto err_mutex;
1514
1515         ov965x->pad.flags = MEDIA_PAD_FL_SOURCE;
1516         sd->entity.function = MEDIA_ENT_F_CAM_SENSOR;
1517         ret = media_entity_pads_init(&sd->entity, 1, &ov965x->pad);
1518         if (ret < 0)
1519                 goto err_mutex;
1520
1521         ret = ov965x_initialize_controls(ov965x);
1522         if (ret < 0)
1523                 goto err_me;
1524
1525         ov965x_get_default_format(&ov965x->format);
1526         ov965x->frame_size = &ov965x_framesizes[0];
1527         ov965x->fiv = &ov965x_intervals[0];
1528
1529         ret = ov965x_detect_sensor(sd);
1530         if (ret < 0)
1531                 goto err_ctrls;
1532
1533         /* Update exposure time min/max to match frame format */
1534         ov965x_update_exposure_ctrl(ov965x);
1535
1536         ret = v4l2_async_register_subdev(sd);
1537         if (ret < 0)
1538                 goto err_ctrls;
1539
1540         return 0;
1541 err_ctrls:
1542         v4l2_ctrl_handler_free(sd->ctrl_handler);
1543 err_me:
1544         media_entity_cleanup(&sd->entity);
1545 err_mutex:
1546         mutex_destroy(&ov965x->lock);
1547         return ret;
1548 }
1549
1550 static int ov965x_remove(struct i2c_client *client)
1551 {
1552         struct v4l2_subdev *sd = i2c_get_clientdata(client);
1553         struct ov965x *ov965x = to_ov965x(sd);
1554
1555         v4l2_async_unregister_subdev(sd);
1556         v4l2_ctrl_handler_free(sd->ctrl_handler);
1557         media_entity_cleanup(&sd->entity);
1558         mutex_destroy(&ov965x->lock);
1559
1560         return 0;
1561 }
1562
1563 static const struct i2c_device_id ov965x_id[] = {
1564         { "OV9650", 0 },
1565         { "OV9652", 0 },
1566         { /* sentinel */ }
1567 };
1568 MODULE_DEVICE_TABLE(i2c, ov965x_id);
1569
1570 static struct i2c_driver ov965x_i2c_driver = {
1571         .driver = {
1572                 .name   = DRIVER_NAME,
1573         },
1574         .probe          = ov965x_probe,
1575         .remove         = ov965x_remove,
1576         .id_table       = ov965x_id,
1577 };
1578
1579 module_i2c_driver(ov965x_i2c_driver);
1580
1581 MODULE_AUTHOR("Sylwester Nawrocki <sylvester.nawrocki@gmail.com>");
1582 MODULE_DESCRIPTION("OV9650/OV9652 CMOS Image Sensor driver");
1583 MODULE_LICENSE("GPL");