drivers/media/i2c/ov9650.c

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