kernel/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                 rgain = (gain - ((1 << m) * 16)) / (1 << m);
 711                 rgain |= (((1 << m) - 1) << 4);
 712
 713                 ret = ov965x_write(client, REG_GAIN, rgain & 0xff);
 714                 if (ret < 0)
 715                         return ret;
 716                 ret = ov965x_read(client, REG_VREF, &reg);
 717                 if (ret < 0)
 718                         return ret;
 719                 reg &= ~VREF_GAIN_MASK;
 720                 reg |= (((rgain >> 8) & 0x3) << 6);
 721                 ret = ov965x_write(client, REG_VREF, reg);
 722                 if (ret < 0)
 723                         return ret;
 724                 /* Return updated control's value to userspace */
 725                 ctrls->gain->val = (1 << m) * (16 + (rgain & 0xf));
 726         }
 727
 728         return ret;
 729 }
 730
 731 static int ov965x_set_sharpness(struct ov965x *ov965x, unsigned int value)
 732 {
 733         u8 com14, edge;
 734         int ret;
 735
 736         ret = ov965x_read(ov965x->client, REG_COM14, &com14);
 737         if (ret < 0)
 738                 return ret;
 739         ret = ov965x_read(ov965x->client, REG_EDGE, &edge);
 740         if (ret < 0)
 741                 return ret;
 742         com14 = value ? com14 | COM14_EDGE_EN : com14 & ~COM14_EDGE_EN;
 743         value--;
 744         if (value > 0x0f) {
 745                 com14 |= COM14_EEF_X2;
 746                 value >>= 1;
 747         } else {
 748                 com14 &= ~COM14_EEF_X2;
 749         }
 750         ret = ov965x_write(ov965x->client, REG_COM14, com14);
 751         if (ret < 0)
 752                 return ret;
 753
 754         edge &= ~EDGE_FACTOR_MASK;
 755         edge |= ((u8)value & 0x0f);
 756
 757         return ov965x_write(ov965x->client, REG_EDGE, edge);
 758 }
 759
 760 static int ov965x_set_exposure(struct ov965x *ov965x, int exp)
 761 {
 762         struct i2c_client *client = ov965x->client;
 763         struct ov965x_ctrls *ctrls = &ov965x->ctrls;
 764         bool auto_exposure = (exp == V4L2_EXPOSURE_AUTO);
 765         int ret;
 766         u8 reg;
 767
 768         if (ctrls->auto_exp->is_new) {
 769                 ret = ov965x_read(client, REG_COM8, &reg);
 770                 if (ret < 0)
 771                         return ret;
 772                 if (auto_exposure)
 773                         reg |= (COM8_AEC | COM8_AGC);
 774                 else
 775                         reg &= ~(COM8_AEC | COM8_AGC);
 776                 ret = ov965x_write(client, REG_COM8, reg);
 777                 if (ret < 0)
 778                         return ret;
 779         }
 780
 781         if (!auto_exposure && ctrls->exposure->is_new) {
 782                 unsigned int exposure = (ctrls->exposure->val * 100)
 783                                          / ov965x->exp_row_interval;
 784                 /*
 785                  * Manual exposure value
 786                  * [b15:b0] - AECHM (b15:b10), AECH (b9:b2), COM1 (b1:b0)
 787                  */
 788                 ret = ov965x_write(client, REG_COM1, exposure & 0x3);
 789                 if (!ret)
 790                         ret = ov965x_write(client, REG_AECH,
 791                                            (exposure >> 2) & 0xff);
 792                 if (!ret)
 793                         ret = ov965x_write(client, REG_AECHM,
 794                                            (exposure >> 10) & 0x3f);
 795                 /* Update the value to minimize rounding errors */
 796                 ctrls->exposure->val = ((exposure * ov965x->exp_row_interval)
 797                                                         + 50) / 100;
 798                 if (ret < 0)
 799                         return ret;
 800         }
 801
 802         v4l2_ctrl_activate(ov965x->ctrls.brightness, !exp);
 803         return 0;
 804 }
 805
 806 static int ov965x_set_flip(struct ov965x *ov965x)
 807 {
 808         u8 mvfp = 0;
 809
 810         if (ov965x->ctrls.hflip->val)
 811                 mvfp |= MVFP_MIRROR;
 812
 813         if (ov965x->ctrls.vflip->val)
 814                 mvfp |= MVFP_FLIP;
 815
 816         return ov965x_write(ov965x->client, REG_MVFP, mvfp);
 817 }
 818
 819 #define NUM_SAT_LEVELS  5
 820 #define NUM_SAT_REGS    6
 821
 822 static int ov965x_set_saturation(struct ov965x *ov965x, int val)
 823 {
 824         static const u8 regs[NUM_SAT_LEVELS][NUM_SAT_REGS] = {
 825                 /* MTX(1)...MTX(6) */
 826                 { 0x1d, 0x1f, 0x02, 0x09, 0x13, 0x1c }, /* -2 */
 827                 { 0x2e, 0x31, 0x02, 0x0e, 0x1e, 0x2d }, /* -1 */
 828                 { 0x3a, 0x3d, 0x03, 0x12, 0x26, 0x38 }, /*  0 */
 829                 { 0x46, 0x49, 0x04, 0x16, 0x2e, 0x43 }, /* +1 */
 830                 { 0x57, 0x5c, 0x05, 0x1b, 0x39, 0x54 }, /* +2 */
 831         };
 832         u8 addr = REG_MTX(1);
 833         int i, ret = 0;
 834
 835         val += (NUM_SAT_LEVELS / 2);
 836         if (val >= NUM_SAT_LEVELS)
 837                 return -EINVAL;
 838
 839         for (i = 0; i < NUM_SAT_REGS && !ret; i++)
 840                 ret = ov965x_write(ov965x->client, addr + i, regs[val][i]);
 841
 842         return ret;
 843 }
 844
 845 static int ov965x_set_test_pattern(struct ov965x *ov965x, int value)
 846 {
 847         int ret;
 848         u8 reg;
 849
 850         ret = ov965x_read(ov965x->client, REG_COM23, &reg);
 851         if (ret < 0)
 852                 return ret;
 853         reg = value ? reg | COM23_TEST_MODE : reg & ~COM23_TEST_MODE;
 854         return ov965x_write(ov965x->client, REG_COM23, reg);
 855 }
 856
 857 static int __g_volatile_ctrl(struct ov965x *ov965x, struct v4l2_ctrl *ctrl)
 858 {
 859         struct i2c_client *client = ov965x->client;
 860         unsigned int exposure, gain, m;
 861         u8 reg0, reg1, reg2;
 862         int ret;
 863
 864         if (!ov965x->power)
 865                 return 0;
 866
 867         switch (ctrl->id) {
 868         case V4L2_CID_AUTOGAIN:
 869                 if (!ctrl->val)
 870                         return 0;
 871                 ret = ov965x_read(client, REG_GAIN, &reg0);
 872                 if (ret < 0)
 873                         return ret;
 874                 ret = ov965x_read(client, REG_VREF, &reg1);
 875                 if (ret < 0)
 876                         return ret;
 877                 gain = ((reg1 >> 6) << 8) | reg0;
 878                 m = 0x01 << fls(gain >> 4);
 879                 ov965x->ctrls.gain->val = m * (16 + (gain & 0xf));
 880                 break;
 881
 882         case V4L2_CID_EXPOSURE_AUTO:
 883                 if (ctrl->val == V4L2_EXPOSURE_MANUAL)
 884                         return 0;
 885                 ret = ov965x_read(client, REG_COM1, &reg0);
 886                 if (!ret)
 887                         ret = ov965x_read(client, REG_AECH, &reg1);
 888                 if (!ret)
 889                         ret = ov965x_read(client, REG_AECHM, &reg2);
 890                 if (ret < 0)
 891                         return ret;
 892                 exposure = ((reg2 & 0x3f) << 10) | (reg1 << 2) |
 893                                                 (reg0 & 0x3);
 894                 ov965x->ctrls.exposure->val = ((exposure *
 895                                 ov965x->exp_row_interval) + 50) / 100;
 896                 break;
 897         }
 898
 899         return 0;
 900 }
 901
 902 static int ov965x_g_volatile_ctrl(struct v4l2_ctrl *ctrl)
 903 {
 904         struct v4l2_subdev *sd = ctrl_to_sd(ctrl);
 905         struct ov965x *ov965x = to_ov965x(sd);
 906         int ret;
 907
 908         v4l2_dbg(1, debug, sd, "g_ctrl: %s\n", ctrl->name);
 909
 910         mutex_lock(&ov965x->lock);
 911         ret = __g_volatile_ctrl(ov965x, ctrl);
 912         mutex_unlock(&ov965x->lock);
 913         return ret;
 914 }
 915
 916 static int ov965x_s_ctrl(struct v4l2_ctrl *ctrl)
 917 {
 918         struct v4l2_subdev *sd = ctrl_to_sd(ctrl);
 919         struct ov965x *ov965x = to_ov965x(sd);
 920         int ret = -EINVAL;
 921
 922         v4l2_dbg(1, debug, sd, "s_ctrl: %s, value: %d. power: %d\n",
 923                  ctrl->name, ctrl->val, ov965x->power);
 924
 925         mutex_lock(&ov965x->lock);
 926         /*
 927          * If the device is not powered up now postpone applying control's
 928          * value to the hardware, until it is ready to accept commands.
 929          */
 930         if (ov965x->power == 0) {
 931                 mutex_unlock(&ov965x->lock);
 932                 return 0;
 933         }
 934
 935         switch (ctrl->id) {
 936         case V4L2_CID_AUTO_WHITE_BALANCE:
 937                 ret = ov965x_set_white_balance(ov965x, ctrl->val);
 938                 break;
 939
 940         case V4L2_CID_BRIGHTNESS:
 941                 ret = ov965x_set_brightness(ov965x, ctrl->val);
 942                 break;
 943
 944         case V4L2_CID_EXPOSURE_AUTO:
 945                 ret = ov965x_set_exposure(ov965x, ctrl->val);
 946                 break;
 947
 948         case V4L2_CID_AUTOGAIN:
 949                 ret = ov965x_set_gain(ov965x, ctrl->val);
 950                 break;
 951
 952         case V4L2_CID_HFLIP:
 953                 ret = ov965x_set_flip(ov965x);
 954                 break;
 955
 956         case V4L2_CID_POWER_LINE_FREQUENCY:
 957                 ret = ov965x_set_banding_filter(ov965x, ctrl->val);
 958                 break;
 959
 960         case V4L2_CID_SATURATION:
 961                 ret = ov965x_set_saturation(ov965x, ctrl->val);
 962                 break;
 963
 964         case V4L2_CID_SHARPNESS:
 965                 ret = ov965x_set_sharpness(ov965x, ctrl->val);
 966                 break;
 967
 968         case V4L2_CID_TEST_PATTERN:
 969                 ret = ov965x_set_test_pattern(ov965x, ctrl->val);
 970                 break;
 971         }
 972
 973         mutex_unlock(&ov965x->lock);
 974         return ret;
 975 }
 976
 977 static const struct v4l2_ctrl_ops ov965x_ctrl_ops = {
 978         .g_volatile_ctrl = ov965x_g_volatile_ctrl,
 979         .s_ctrl = ov965x_s_ctrl,
 980 };
 981
 982 static const char * const test_pattern_menu[] = {
 983         "Disabled",
 984         "Color bars",
 985         NULL
 986 };
 987
 988 static int ov965x_initialize_controls(struct ov965x *ov965x)
 989 {
 990         const struct v4l2_ctrl_ops *ops = &ov965x_ctrl_ops;
 991         struct ov965x_ctrls *ctrls = &ov965x->ctrls;
 992         struct v4l2_ctrl_handler *hdl = &ctrls->handler;
 993         int ret;
 994
 995         ret = v4l2_ctrl_handler_init(hdl, 16);
 996         if (ret < 0)
 997                 return ret;
 998
 999         /* Auto/manual white balance */
1000         ctrls->auto_wb = v4l2_ctrl_new_std(hdl, ops,
1001                                 V4L2_CID_AUTO_WHITE_BALANCE,
1002                                 0, 1, 1, 1);
1003         ctrls->blue_balance = v4l2_ctrl_new_std(hdl, ops, V4L2_CID_BLUE_BALANCE,
1004                                                 0, 0xff, 1, 0x80);
1005         ctrls->red_balance = v4l2_ctrl_new_std(hdl, ops, V4L2_CID_RED_BALANCE,
1006                                                 0, 0xff, 1, 0x80);
1007         /* Auto/manual exposure */
1008         ctrls->auto_exp = v4l2_ctrl_new_std_menu(hdl, ops,
1009                                 V4L2_CID_EXPOSURE_AUTO,
1010                                 V4L2_EXPOSURE_MANUAL, 0, V4L2_EXPOSURE_AUTO);
1011         /* Exposure time, in 100 us units. min/max is updated dynamically. */
1012         ctrls->exposure = v4l2_ctrl_new_std(hdl, ops,
1013                                 V4L2_CID_EXPOSURE_ABSOLUTE,
1014                                 2, 1500, 1, 500);
1015         /* Auto/manual gain */
1016         ctrls->auto_gain = v4l2_ctrl_new_std(hdl, ops, V4L2_CID_AUTOGAIN,
1017                                                 0, 1, 1, 1);
1018         ctrls->gain = v4l2_ctrl_new_std(hdl, ops, V4L2_CID_GAIN,
1019                                                 16, 64 * (16 + 15), 1, 64 * 16);
1020
1021         ctrls->saturation = v4l2_ctrl_new_std(hdl, ops, V4L2_CID_SATURATION,
1022                                                 -2, 2, 1, 0);
1023         ctrls->brightness = v4l2_ctrl_new_std(hdl, ops, V4L2_CID_BRIGHTNESS,
1024                                                 -3, 3, 1, 0);
1025         ctrls->sharpness = v4l2_ctrl_new_std(hdl, ops, V4L2_CID_SHARPNESS,
1026                                                 0, 32, 1, 6);
1027
1028         ctrls->hflip = v4l2_ctrl_new_std(hdl, ops, V4L2_CID_HFLIP, 0, 1, 1, 0);
1029         ctrls->vflip = v4l2_ctrl_new_std(hdl, ops, V4L2_CID_VFLIP, 0, 1, 1, 0);
1030
1031         ctrls->light_freq = v4l2_ctrl_new_std_menu(hdl, ops,
1032                                 V4L2_CID_POWER_LINE_FREQUENCY,
1033                                 V4L2_CID_POWER_LINE_FREQUENCY_60HZ, ~0x7,
1034                                 V4L2_CID_POWER_LINE_FREQUENCY_50HZ);
1035
1036         v4l2_ctrl_new_std_menu_items(hdl, ops, V4L2_CID_TEST_PATTERN,
1037                                 ARRAY_SIZE(test_pattern_menu) - 1, 0, 0,
1038                                 test_pattern_menu);
1039         if (hdl->error) {
1040                 ret = hdl->error;
1041                 v4l2_ctrl_handler_free(hdl);
1042                 return ret;
1043         }
1044
1045         ctrls->gain->flags |= V4L2_CTRL_FLAG_VOLATILE;
1046         ctrls->exposure->flags |= V4L2_CTRL_FLAG_VOLATILE;
1047
1048         v4l2_ctrl_auto_cluster(3, &ctrls->auto_wb, 0, false);
1049         v4l2_ctrl_auto_cluster(3, &ctrls->auto_gain, 0, true);
1050         v4l2_ctrl_auto_cluster(3, &ctrls->auto_exp, 1, true);
1051         v4l2_ctrl_cluster(2, &ctrls->hflip);
1052
1053         ov965x->sd.ctrl_handler = hdl;
1054         return 0;
1055 }
1056
1057 /*
1058  * V4L2 subdev video and pad level operations
1059  */
1060 static void ov965x_get_default_format(struct v4l2_mbus_framefmt *mf)
1061 {
1062         mf->width = ov965x_framesizes[0].width;
1063         mf->height = ov965x_framesizes[0].height;
1064         mf->colorspace = ov965x_formats[0].colorspace;
1065         mf->code = ov965x_formats[0].code;
1066         mf->field = V4L2_FIELD_NONE;
1067 }
1068
1069 static int ov965x_enum_mbus_code(struct v4l2_subdev *sd,
1070                                  struct v4l2_subdev_pad_config *cfg,
1071                                  struct v4l2_subdev_mbus_code_enum *code)
1072 {
1073         if (code->index >= ARRAY_SIZE(ov965x_formats))
1074                 return -EINVAL;
1075
1076         code->code = ov965x_formats[code->index].code;
1077         return 0;
1078 }
1079
1080 static int ov965x_enum_frame_sizes(struct v4l2_subdev *sd,
1081                                    struct v4l2_subdev_pad_config *cfg,
1082                                    struct v4l2_subdev_frame_size_enum *fse)
1083 {
1084         int i = ARRAY_SIZE(ov965x_formats);
1085
1086         if (fse->index >= ARRAY_SIZE(ov965x_framesizes))
1087                 return -EINVAL;
1088
1089         while (--i)
1090                 if (fse->code == ov965x_formats[i].code)
1091                         break;
1092
1093         fse->code = ov965x_formats[i].code;
1094
1095         fse->min_width  = ov965x_framesizes[fse->index].width;
1096         fse->max_width  = fse->min_width;
1097         fse->max_height = ov965x_framesizes[fse->index].height;
1098         fse->min_height = fse->max_height;
1099
1100         return 0;
1101 }
1102
1103 static int ov965x_g_frame_interval(struct v4l2_subdev *sd,
1104                                    struct v4l2_subdev_frame_interval *fi)
1105 {
1106         struct ov965x *ov965x = to_ov965x(sd);
1107
1108         mutex_lock(&ov965x->lock);
1109         fi->interval = ov965x->fiv->interval;
1110         mutex_unlock(&ov965x->lock);
1111
1112         return 0;
1113 }
1114
1115 static int __ov965x_set_frame_interval(struct ov965x *ov965x,
1116                                        struct v4l2_subdev_frame_interval *fi)
1117 {
1118         struct v4l2_mbus_framefmt *mbus_fmt = &ov965x->format;
1119         const struct ov965x_interval *fiv = &ov965x_intervals[0];
1120         u64 req_int, err, min_err = ~0ULL;
1121         unsigned int i;
1122
1123
1124         if (fi->interval.denominator == 0)
1125                 return -EINVAL;
1126
1127         req_int = (u64)(fi->interval.numerator * 10000) /
1128                 fi->interval.denominator;
1129
1130         for (i = 0; i < ARRAY_SIZE(ov965x_intervals); i++) {
1131                 const struct ov965x_interval *iv = &ov965x_intervals[i];
1132
1133                 if (mbus_fmt->width != iv->size.width ||
1134                     mbus_fmt->height != iv->size.height)
1135                         continue;
1136                 err = abs64((u64)(iv->interval.numerator * 10000) /
1137                             iv->interval.denominator - req_int);
1138                 if (err < min_err) {
1139                         fiv = iv;
1140                         min_err = err;
1141                 }
1142         }
1143         ov965x->fiv = fiv;
1144
1145         v4l2_dbg(1, debug, &ov965x->sd, "Changed frame interval to %u us\n",
1146                  fiv->interval.numerator * 1000000 / fiv->interval.denominator);
1147
1148         return 0;
1149 }
1150
1151 static int ov965x_s_frame_interval(struct v4l2_subdev *sd,
1152                                    struct v4l2_subdev_frame_interval *fi)
1153 {
1154         struct ov965x *ov965x = to_ov965x(sd);
1155         int ret;
1156
1157         v4l2_dbg(1, debug, sd, "Setting %d/%d frame interval\n",
1158                  fi->interval.numerator, fi->interval.denominator);
1159
1160         mutex_lock(&ov965x->lock);
1161         ret = __ov965x_set_frame_interval(ov965x, fi);
1162         ov965x->apply_frame_fmt = 1;
1163         mutex_unlock(&ov965x->lock);
1164         return ret;
1165 }
1166
1167 static int ov965x_get_fmt(struct v4l2_subdev *sd, struct v4l2_subdev_pad_config *cfg,
1168                           struct v4l2_subdev_format *fmt)
1169 {
1170         struct ov965x *ov965x = to_ov965x(sd);
1171         struct v4l2_mbus_framefmt *mf;
1172
1173         if (fmt->which == V4L2_SUBDEV_FORMAT_TRY) {
1174                 mf = v4l2_subdev_get_try_format(sd, cfg, 0);
1175                 fmt->format = *mf;
1176                 return 0;
1177         }
1178
1179         mutex_lock(&ov965x->lock);
1180         fmt->format = ov965x->format;
1181         mutex_unlock(&ov965x->lock);
1182
1183         return 0;
1184 }
1185
1186 static void __ov965x_try_frame_size(struct v4l2_mbus_framefmt *mf,
1187                                     const struct ov965x_framesize **size)
1188 {
1189         const struct ov965x_framesize *fsize = &ov965x_framesizes[0],
1190                 *match = NULL;
1191         int i = ARRAY_SIZE(ov965x_framesizes);
1192         unsigned int min_err = UINT_MAX;
1193
1194         while (i--) {
1195                 int err = abs(fsize->width - mf->width)
1196                                 + abs(fsize->height - mf->height);
1197                 if (err < min_err) {
1198                         min_err = err;
1199                         match = fsize;
1200                 }
1201                 fsize++;
1202         }
1203         if (!match)
1204                 match = &ov965x_framesizes[0];
1205         mf->width  = match->width;
1206         mf->height = match->height;
1207         if (size)
1208                 *size = match;
1209 }
1210
1211 static int ov965x_set_fmt(struct v4l2_subdev *sd, struct v4l2_subdev_pad_config *cfg,
1212                           struct v4l2_subdev_format *fmt)
1213 {
1214         unsigned int index = ARRAY_SIZE(ov965x_formats);
1215         struct v4l2_mbus_framefmt *mf = &fmt->format;
1216         struct ov965x *ov965x = to_ov965x(sd);
1217         const struct ov965x_framesize *size = NULL;
1218         int ret = 0;
1219
1220         __ov965x_try_frame_size(mf, &size);
1221
1222         while (--index)
1223                 if (ov965x_formats[index].code == mf->code)
1224                         break;
1225
1226         mf->colorspace  = V4L2_COLORSPACE_JPEG;
1227         mf->code        = ov965x_formats[index].code;
1228         mf->field       = V4L2_FIELD_NONE;
1229
1230         mutex_lock(&ov965x->lock);
1231
1232         if (fmt->which == V4L2_SUBDEV_FORMAT_TRY) {
1233                 if (cfg != NULL) {
1234                         mf = v4l2_subdev_get_try_format(sd, cfg, fmt->pad);
1235                         *mf = fmt->format;
1236                 }
1237         } else {
1238                 if (ov965x->streaming) {
1239                         ret = -EBUSY;
1240                 } else {
1241                         ov965x->frame_size = size;
1242                         ov965x->format = fmt->format;
1243                         ov965x->tslb_reg = ov965x_formats[index].tslb_reg;
1244                         ov965x->apply_frame_fmt = 1;
1245                 }
1246         }
1247
1248         if (!ret && fmt->which == V4L2_SUBDEV_FORMAT_ACTIVE) {
1249                 struct v4l2_subdev_frame_interval fiv = {
1250                         .interval = { 0, 1 }
1251                 };
1252                 /* Reset to minimum possible frame interval */
1253                 __ov965x_set_frame_interval(ov965x, &fiv);
1254         }
1255         mutex_unlock(&ov965x->lock);
1256
1257         if (!ret)
1258                 ov965x_update_exposure_ctrl(ov965x);
1259
1260         return ret;
1261 }
1262
1263 static int ov965x_set_frame_size(struct ov965x *ov965x)
1264 {
1265         int i, ret = 0;
1266
1267         for (i = 0; ret == 0 && i < NUM_FMT_REGS; i++)
1268                 ret = ov965x_write(ov965x->client, frame_size_reg_addr[i],
1269                                    ov965x->frame_size->regs[i]);
1270         return ret;
1271 }
1272
1273 static int __ov965x_set_params(struct ov965x *ov965x)
1274 {
1275         struct i2c_client *client = ov965x->client;
1276         struct ov965x_ctrls *ctrls = &ov965x->ctrls;
1277         int ret = 0;
1278         u8 reg;
1279
1280         if (ov965x->apply_frame_fmt) {
1281                 reg = DEF_CLKRC + ov965x->fiv->clkrc_div;
1282                 ret = ov965x_write(client, REG_CLKRC, reg);
1283                 if (ret < 0)
1284                         return ret;
1285                 ret = ov965x_set_frame_size(ov965x);
1286                 if (ret < 0)
1287                         return ret;
1288                 ret = ov965x_read(client, REG_TSLB, &reg);
1289                 if (ret < 0)
1290                         return ret;
1291                 reg &= ~TSLB_YUYV_MASK;
1292                 reg |= ov965x->tslb_reg;
1293                 ret = ov965x_write(client, REG_TSLB, reg);
1294                 if (ret < 0)
1295                         return ret;
1296         }
1297         ret = ov965x_set_default_gamma_curve(ov965x);
1298         if (ret < 0)
1299                 return ret;
1300         ret = ov965x_set_color_matrix(ov965x);
1301         if (ret < 0)
1302                 return ret;
1303         /*
1304          * Select manual banding filter, the filter will
1305          * be enabled further if required.
1306          */
1307         ret = ov965x_read(client, REG_COM11, &reg);
1308         if (!ret)
1309                 reg |= COM11_BANDING;
1310         ret = ov965x_write(client, REG_COM11, reg);
1311         if (ret < 0)
1312                 return ret;
1313         /*
1314          * Banding filter (REG_MBD value) needs to match selected
1315          * resolution and frame rate, so it's always updated here.
1316          */
1317         return ov965x_set_banding_filter(ov965x, ctrls->light_freq->val);
1318 }
1319
1320 static int ov965x_s_stream(struct v4l2_subdev *sd, int on)
1321 {
1322         struct i2c_client *client = v4l2_get_subdevdata(sd);
1323         struct ov965x *ov965x = to_ov965x(sd);
1324         struct ov965x_ctrls *ctrls = &ov965x->ctrls;
1325         int ret = 0;
1326
1327         v4l2_dbg(1, debug, client, "%s: on: %d\n", __func__, on);
1328
1329         mutex_lock(&ov965x->lock);
1330         if (ov965x->streaming == !on) {
1331                 if (on)
1332                         ret = __ov965x_set_params(ov965x);
1333
1334                 if (!ret && ctrls->update) {
1335                         /*
1336                          * ov965x_s_ctrl callback takes the mutex
1337                          * so it needs to be released here.
1338                          */
1339                         mutex_unlock(&ov965x->lock);
1340                         ret = v4l2_ctrl_handler_setup(&ctrls->handler);
1341
1342                         mutex_lock(&ov965x->lock);
1343                         if (!ret)
1344                                 ctrls->update = 0;
1345                 }
1346                 if (!ret)
1347                         ret = ov965x_write(client, REG_COM2,
1348                                            on ? 0x01 : 0x11);
1349         }
1350         if (!ret)
1351                 ov965x->streaming += on ? 1 : -1;
1352
1353         WARN_ON(ov965x->streaming < 0);
1354         mutex_unlock(&ov965x->lock);
1355
1356         return ret;
1357 }
1358
1359 /*
1360  * V4L2 subdev internal operations
1361  */
1362 static int ov965x_open(struct v4l2_subdev *sd, struct v4l2_subdev_fh *fh)
1363 {
1364         struct v4l2_mbus_framefmt *mf = v4l2_subdev_get_try_format(sd, fh->pad, 0);
1365
1366         ov965x_get_default_format(mf);
1367         return 0;
1368 }
1369
1370 static const struct v4l2_subdev_pad_ops ov965x_pad_ops = {
1371         .enum_mbus_code = ov965x_enum_mbus_code,
1372         .enum_frame_size = ov965x_enum_frame_sizes,
1373         .get_fmt = ov965x_get_fmt,
1374         .set_fmt = ov965x_set_fmt,
1375 };
1376
1377 static const struct v4l2_subdev_video_ops ov965x_video_ops = {
1378         .s_stream = ov965x_s_stream,
1379         .g_frame_interval = ov965x_g_frame_interval,
1380         .s_frame_interval = ov965x_s_frame_interval,
1381
1382 };
1383
1384 static const struct v4l2_subdev_internal_ops ov965x_sd_internal_ops = {
1385         .open = ov965x_open,
1386 };
1387
1388 static const struct v4l2_subdev_core_ops ov965x_core_ops = {
1389         .s_power = ov965x_s_power,
1390         .log_status = v4l2_ctrl_subdev_log_status,
1391         .subscribe_event = v4l2_ctrl_subdev_subscribe_event,
1392         .unsubscribe_event = v4l2_event_subdev_unsubscribe,
1393 };
1394
1395 static const struct v4l2_subdev_ops ov965x_subdev_ops = {
1396         .core = &ov965x_core_ops,
1397         .pad = &ov965x_pad_ops,
1398         .video = &ov965x_video_ops,
1399 };
1400
1401 /*
1402  * Reset and power down GPIOs configuration
1403  */
1404 static int ov965x_configure_gpios(struct ov965x *ov965x,
1405                                   const struct ov9650_platform_data *pdata)
1406 {
1407         int ret, i;
1408
1409         ov965x->gpios[GPIO_PWDN] = pdata->gpio_pwdn;
1410         ov965x->gpios[GPIO_RST]  = pdata->gpio_reset;
1411
1412         for (i = 0; i < ARRAY_SIZE(ov965x->gpios); i++) {
1413                 int gpio = ov965x->gpios[i];
1414
1415                 if (!gpio_is_valid(gpio))
1416                         continue;
1417                 ret = devm_gpio_request_one(&ov965x->client->dev, gpio,
1418                                             GPIOF_OUT_INIT_HIGH, "OV965X");
1419                 if (ret < 0)
1420                         return ret;
1421                 v4l2_dbg(1, debug, &ov965x->sd, "set gpio %d to 1\n", gpio);
1422
1423                 gpio_set_value(gpio, 1);
1424                 gpio_export(gpio, 0);
1425                 ov965x->gpios[i] = gpio;
1426         }
1427
1428         return 0;
1429 }
1430
1431 static int ov965x_detect_sensor(struct v4l2_subdev *sd)
1432 {
1433         struct i2c_client *client = v4l2_get_subdevdata(sd);
1434         struct ov965x *ov965x = to_ov965x(sd);
1435         u8 pid, ver;
1436         int ret;
1437
1438         mutex_lock(&ov965x->lock);
1439          __ov965x_set_power(ov965x, 1);
1440         usleep_range(25000, 26000);
1441
1442         /* Check sensor revision */
1443         ret = ov965x_read(client, REG_PID, &pid);
1444         if (!ret)
1445                 ret = ov965x_read(client, REG_VER, &ver);
1446
1447         __ov965x_set_power(ov965x, 0);
1448
1449         if (!ret) {
1450                 ov965x->id = OV965X_ID(pid, ver);
1451                 if (ov965x->id == OV9650_ID || ov965x->id == OV9652_ID) {
1452                         v4l2_info(sd, "Found OV%04X sensor\n", ov965x->id);
1453                 } else {
1454                         v4l2_err(sd, "Sensor detection failed (%04X, %d)\n",
1455                                  ov965x->id, ret);
1456                         ret = -ENODEV;
1457                 }
1458         }
1459         mutex_unlock(&ov965x->lock);
1460
1461         return ret;
1462 }
1463
1464 static int ov965x_probe(struct i2c_client *client,
1465                         const struct i2c_device_id *id)
1466 {
1467         const struct ov9650_platform_data *pdata = client->dev.platform_data;
1468         struct v4l2_subdev *sd;
1469         struct ov965x *ov965x;
1470         int ret;
1471
1472         if (pdata == NULL) {
1473                 dev_err(&client->dev, "platform data not specified\n");
1474                 return -EINVAL;
1475         }
1476
1477         if (pdata->mclk_frequency == 0) {
1478                 dev_err(&client->dev, "MCLK frequency not specified\n");
1479                 return -EINVAL;
1480         }
1481
1482         ov965x = devm_kzalloc(&client->dev, sizeof(*ov965x), GFP_KERNEL);
1483         if (!ov965x)
1484                 return -ENOMEM;
1485
1486         mutex_init(&ov965x->lock);
1487         ov965x->client = client;
1488         ov965x->mclk_frequency = pdata->mclk_frequency;
1489
1490         sd = &ov965x->sd;
1491         v4l2_i2c_subdev_init(sd, client, &ov965x_subdev_ops);
1492         strlcpy(sd->name, DRIVER_NAME, sizeof(sd->name));
1493
1494         sd->internal_ops = &ov965x_sd_internal_ops;
1495         sd->flags |= V4L2_SUBDEV_FL_HAS_DEVNODE |
1496                      V4L2_SUBDEV_FL_HAS_EVENTS;
1497
1498         ret = ov965x_configure_gpios(ov965x, pdata);
1499         if (ret < 0)
1500                 return ret;
1501
1502         ov965x->pad.flags = MEDIA_PAD_FL_SOURCE;
1503         sd->entity.type = MEDIA_ENT_T_V4L2_SUBDEV_SENSOR;
1504         ret = media_entity_init(&sd->entity, 1, &ov965x->pad, 0);
1505         if (ret < 0)
1506                 return ret;
1507
1508         ret = ov965x_initialize_controls(ov965x);
1509         if (ret < 0)
1510                 goto err_me;
1511
1512         ov965x_get_default_format(&ov965x->format);
1513         ov965x->frame_size = &ov965x_framesizes[0];
1514         ov965x->fiv = &ov965x_intervals[0];
1515
1516         ret = ov965x_detect_sensor(sd);
1517         if (ret < 0)
1518                 goto err_ctrls;
1519
1520         /* Update exposure time min/max to match frame format */
1521         ov965x_update_exposure_ctrl(ov965x);
1522
1523         return 0;
1524 err_ctrls:
1525         v4l2_ctrl_handler_free(sd->ctrl_handler);
1526 err_me:
1527         media_entity_cleanup(&sd->entity);
1528         return ret;
1529 }
1530
1531 static int ov965x_remove(struct i2c_client *client)
1532 {
1533         struct v4l2_subdev *sd = i2c_get_clientdata(client);
1534
1535         v4l2_device_unregister_subdev(sd);
1536         v4l2_ctrl_handler_free(sd->ctrl_handler);
1537         media_entity_cleanup(&sd->entity);
1538
1539         return 0;
1540 }
1541
1542 static const struct i2c_device_id ov965x_id[] = {
1543         { "OV9650", 0 },
1544         { "OV9652", 0 },
1545         { /* sentinel */ }
1546 };
1547 MODULE_DEVICE_TABLE(i2c, ov965x_id);
1548
1549 static struct i2c_driver ov965x_i2c_driver = {
1550         .driver = {
1551                 .name   = DRIVER_NAME,
1552         },
1553         .probe          = ov965x_probe,
1554         .remove         = ov965x_remove,
1555         .id_table       = ov965x_id,
1556 };
1557
1558 module_i2c_driver(ov965x_i2c_driver);
1559
1560 MODULE_AUTHOR("Sylwester Nawrocki <sylvester.nawrocki@gmail.com>");
1561 MODULE_DESCRIPTION("OV9650/OV9652 CMOS Image Sensor driver");
1562 MODULE_LICENSE("GPL");