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