drivers/media/i2c/ov9650.c

   1 // SPDX-License-Identifier: GPL-2.0-only
   2 /*
   3  * Omnivision OV9650/OV9652 CMOS Image Sensor driver
   4  *
   5  * Copyright (C) 2013, Sylwester Nawrocki <sylvester.nawrocki@gmail.com>
   6  *
   7  * Register definitions and initial settings based on a driver written
   8  * by Vladimir Fonov.
   9  * Copyright (c) 2010, Vladimir Fonov
  10  */
  11 #include <linux/clk.h>
  12 #include <linux/delay.h>
  13 #include <linux/gpio/consumer.h>
  14 #include <linux/i2c.h>
  15 #include <linux/kernel.h>
  16 #include <linux/media.h>
  17 #include <linux/module.h>
  18 #include <linux/ratelimit.h>
  19 #include <linux/regmap.h>
  20 #include <linux/slab.h>
  21 #include <linux/string.h>
  22 #include <linux/videodev2.h>
  23
  24 #include <media/media-entity.h>
  25 #include <media/v4l2-async.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
  33 static int debug;
  34 module_param(debug, int, 0644);
  35 MODULE_PARM_DESC(debug, "Debug level (0-2)");
  36
  37 #define DRIVER_NAME "OV9650"
  38
  39 /*
  40  * OV9650/OV9652 register definitions
  41  */
  42 #define REG_GAIN                0x00    /* Gain control, AGC[7:0] */
  43 #define REG_BLUE                0x01    /* AWB - Blue channel gain */
  44 #define REG_RED                 0x02    /* AWB - Red channel gain */
  45 #define REG_VREF                0x03    /* [7:6] - AGC[9:8], [5:3]/[2:0] */
  46 #define  VREF_GAIN_MASK         0xc0    /* - VREF end/start low 3 bits */
  47 #define REG_COM1                0x04
  48 #define  COM1_CCIR656           0x40
  49 #define REG_B_AVE               0x05
  50 #define REG_GB_AVE              0x06
  51 #define REG_GR_AVE              0x07
  52 #define REG_R_AVE               0x08
  53 #define REG_COM2                0x09
  54 #define REG_PID                 0x0a    /* Product ID MSB */
  55 #define REG_VER                 0x0b    /* Product ID LSB */
  56 #define REG_COM3                0x0c
  57 #define  COM3_SWAP              0x40
  58 #define  COM3_VARIOPIXEL1       0x04
  59 #define REG_COM4                0x0d    /* Vario Pixels  */
  60 #define  COM4_VARIOPIXEL2       0x80
  61 #define REG_COM5                0x0e    /* System clock options */
  62 #define  COM5_SLAVE_MODE        0x10
  63 #define  COM5_SYSTEMCLOCK48MHZ  0x80
  64 #define REG_COM6                0x0f    /* HREF & ADBLC options */
  65 #define REG_AECH                0x10    /* Exposure value, AEC[9:2] */
  66 #define REG_CLKRC               0x11    /* Clock control */
  67 #define  CLK_EXT                0x40    /* Use external clock directly */
  68 #define  CLK_SCALE              0x3f    /* Mask for internal clock scale */
  69 #define REG_COM7                0x12    /* SCCB reset, output format */
  70 #define  COM7_RESET             0x80
  71 #define  COM7_FMT_MASK          0x38
  72 #define  COM7_FMT_VGA           0x40
  73 #define  COM7_FMT_CIF           0x20
  74 #define  COM7_FMT_QVGA          0x10
  75 #define  COM7_FMT_QCIF          0x08
  76 #define  COM7_RGB               0x04
  77 #define  COM7_YUV               0x00
  78 #define  COM7_BAYER             0x01
  79 #define  COM7_PBAYER            0x05
  80 #define REG_COM8                0x13    /* AGC/AEC options */
  81 #define  COM8_FASTAEC           0x80    /* Enable fast AGC/AEC */
  82 #define  COM8_AECSTEP           0x40    /* Unlimited AEC step size */
  83 #define  COM8_BFILT             0x20    /* Band filter enable */
  84 #define  COM8_AGC               0x04    /* Auto gain enable */
  85 #define  COM8_AWB               0x02    /* White balance enable */
  86 #define  COM8_AEC               0x01    /* Auto exposure enable */
  87 #define REG_COM9                0x14    /* Gain ceiling */
  88 #define  COM9_GAIN_CEIL_MASK    0x70    /* */
  89 #define REG_COM10               0x15    /* PCLK, HREF, HSYNC signals polarity */
  90 #define  COM10_HSYNC            0x40    /* HSYNC instead of HREF */
  91 #define  COM10_PCLK_HB          0x20    /* Suppress PCLK on horiz blank */
  92 #define  COM10_HREF_REV         0x08    /* Reverse HREF */
  93 #define  COM10_VS_LEAD          0x04    /* VSYNC on clock leading edge */
  94 #define  COM10_VS_NEG           0x02    /* VSYNC negative */
  95 #define  COM10_HS_NEG           0x01    /* HSYNC negative */
  96 #define REG_HSTART              0x17    /* Horiz start high bits */
  97 #define REG_HSTOP               0x18    /* Horiz stop high bits */
  98 #define REG_VSTART              0x19    /* Vert start high bits */
  99 #define REG_VSTOP               0x1a    /* Vert stop high bits */
 100 #define REG_PSHFT               0x1b    /* Pixel delay after HREF */
 101 #define REG_MIDH                0x1c    /* Manufacturer ID MSB */
 102 #define REG_MIDL                0x1d    /* Manufufacturer ID LSB */
 103 #define REG_MVFP                0x1e    /* Image mirror/flip */
 104 #define  MVFP_MIRROR            0x20    /* Mirror image */
 105 #define  MVFP_FLIP              0x10    /* Vertical flip */
 106 #define REG_BOS                 0x20    /* B channel Offset */
 107 #define REG_GBOS                0x21    /* Gb channel Offset */
 108 #define REG_GROS                0x22    /* Gr channel Offset */
 109 #define REG_ROS                 0x23    /* R channel Offset */
 110 #define REG_AEW                 0x24    /* AGC upper limit */
 111 #define REG_AEB                 0x25    /* AGC lower limit */
 112 #define REG_VPT                 0x26    /* AGC/AEC fast mode op region */
 113 #define REG_BBIAS               0x27    /* B channel output bias */
 114 #define REG_GBBIAS              0x28    /* Gb channel output bias */
 115 #define REG_GRCOM               0x29    /* Analog BLC & regulator */
 116 #define REG_EXHCH               0x2a    /* Dummy pixel insert MSB */
 117 #define REG_EXHCL               0x2b    /* Dummy pixel insert LSB */
 118 #define REG_RBIAS               0x2c    /* R channel output bias */
 119 #define REG_ADVFL               0x2d    /* LSB of dummy line insert */
 120 #define REG_ADVFH               0x2e    /* MSB of dummy line insert */
 121 #define REG_YAVE                0x2f    /* Y/G channel average value */
 122 #define REG_HSYST               0x30    /* HSYNC rising edge delay LSB*/
 123 #define REG_HSYEN               0x31    /* HSYNC falling edge delay LSB*/
 124 #define REG_HREF                0x32    /* HREF pieces */
 125 #define REG_CHLF                0x33    /* reserved */
 126 #define REG_ADC                 0x37    /* reserved */
 127 #define REG_ACOM                0x38    /* reserved */
 128 #define REG_OFON                0x39    /* Power down register */
 129 #define  OFON_PWRDN             0x08    /* Power down bit */
 130 #define REG_TSLB                0x3a    /* YUVU format */
 131 #define  TSLB_YUYV_MASK         0x0c    /* UYVY or VYUY - see com13 */
 132 #define REG_COM11               0x3b    /* Night mode, banding filter enable */
 133 #define  COM11_NIGHT            0x80    /* Night mode enable */
 134 #define  COM11_NMFR             0x60    /* Two bit NM frame rate */
 135 #define  COM11_BANDING          0x01    /* Banding filter */
 136 #define  COM11_AEC_REF_MASK     0x18    /* AEC reference area selection */
 137 #define REG_COM12               0x3c    /* HREF option, UV average */
 138 #define  COM12_HREF             0x80    /* HREF always */
 139 #define REG_COM13               0x3d    /* Gamma selection, Color matrix en. */
 140 #define  COM13_GAMMA            0x80    /* Gamma enable */
 141 #define  COM13_UVSAT            0x40    /* UV saturation auto adjustment */
 142 #define  COM13_UVSWAP           0x01    /* V before U - w/TSLB */
 143 #define REG_COM14               0x3e    /* Edge enhancement options */
 144 #define  COM14_EDGE_EN          0x02
 145 #define  COM14_EEF_X2           0x01
 146 #define REG_EDGE                0x3f    /* Edge enhancement factor */
 147 #define  EDGE_FACTOR_MASK       0x0f
 148 #define REG_COM15               0x40    /* Output range, RGB 555/565 */
 149 #define  COM15_R10F0            0x00    /* Data range 10 to F0 */
 150 #define  COM15_R01FE            0x80    /* 01 to FE */
 151 #define  COM15_R00FF            0xc0    /* 00 to FF */
 152 #define  COM15_RGB565           0x10    /* RGB565 output */
 153 #define  COM15_RGB555           0x30    /* RGB555 output */
 154 #define  COM15_SWAPRB           0x04    /* Swap R&B */
 155 #define REG_COM16               0x41    /* Color matrix coeff options */
 156 #define REG_COM17               0x42    /* Single frame out, banding filter */
 157 /* n = 1...9, 0x4f..0x57 */
 158 #define REG_MTX(__n)            (0x4f + (__n) - 1)
 159 #define REG_MTXS                0x58
 160 /* Lens Correction Option 1...5, __n = 0...5 */
 161 #define REG_LCC(__n)            (0x62 + (__n) - 1)
 162 #define  LCC5_LCC_ENABLE        0x01    /* LCC5, enable lens correction */
 163 #define  LCC5_LCC_COLOR         0x04
 164 #define REG_MANU                0x67    /* Manual U value */
 165 #define REG_MANV                0x68    /* Manual V value */
 166 #define REG_HV                  0x69    /* Manual banding filter MSB */
 167 #define REG_MBD                 0x6a    /* Manual banding filter value */
 168 #define REG_DBLV                0x6b    /* reserved */
 169 #define REG_GSP                 0x6c    /* Gamma curve */
 170 #define  GSP_LEN                15
 171 #define REG_GST                 0x7c    /* Gamma curve */
 172 #define  GST_LEN                15
 173 #define REG_COM21               0x8b
 174 #define REG_COM22               0x8c    /* Edge enhancement, denoising */
 175 #define  COM22_WHTPCOR          0x02    /* White pixel correction enable */
 176 #define  COM22_WHTPCOROPT       0x01    /* White pixel correction option */
 177 #define  COM22_DENOISE          0x10    /* White pixel correction option */
 178 #define REG_COM23               0x8d    /* Color bar test, color gain */
 179 #define  COM23_TEST_MODE        0x10
 180 #define REG_DBLC1               0x8f    /* Digital BLC */
 181 #define REG_DBLC_B              0x90    /* Digital BLC B channel offset */
 182 #define REG_DBLC_R              0x91    /* Digital BLC R channel offset */
 183 #define REG_DM_LNL              0x92    /* Dummy line low 8 bits */
 184 #define REG_DM_LNH              0x93    /* Dummy line high 8 bits */
 185 #define REG_LCCFB               0x9d    /* Lens Correction B channel */
 186 #define REG_LCCFR               0x9e    /* Lens Correction R channel */
 187 #define REG_DBLC_GB             0x9f    /* Digital BLC GB chan offset */
 188 #define REG_DBLC_GR             0xa0    /* Digital BLC GR chan offset */
 189 #define REG_AECHM               0xa1    /* Exposure value - bits AEC[15:10] */
 190 #define REG_BD50ST              0xa2    /* Banding filter value for 50Hz */
 191 #define REG_BD60ST              0xa3    /* Banding filter value for 60Hz */
 192 #define REG_NULL                0xff    /* Array end token */
 193
 194 #define DEF_CLKRC               0x80
 195
 196 #define OV965X_ID(_msb, _lsb)   ((_msb) << 8 | (_lsb))
 197 #define OV9650_ID               0x9650
 198 #define OV9652_ID               0x9652
 199
 200 struct ov965x_ctrls {
 201         struct v4l2_ctrl_handler handler;
 202         struct {
 203                 struct v4l2_ctrl *auto_exp;
 204                 struct v4l2_ctrl *exposure;
 205         };
 206         struct {
 207                 struct v4l2_ctrl *auto_wb;
 208                 struct v4l2_ctrl *blue_balance;
 209                 struct v4l2_ctrl *red_balance;
 210         };
 211         struct {
 212                 struct v4l2_ctrl *hflip;
 213                 struct v4l2_ctrl *vflip;
 214         };
 215         struct {
 216                 struct v4l2_ctrl *auto_gain;
 217                 struct v4l2_ctrl *gain;
 218         };
 219         struct v4l2_ctrl *brightness;
 220         struct v4l2_ctrl *saturation;
 221         struct v4l2_ctrl *sharpness;
 222         struct v4l2_ctrl *light_freq;
 223         u8 update;
 224 };
 225
 226 struct ov965x_framesize {
 227         u16 width;
 228         u16 height;
 229         u16 max_exp_lines;
 230         const u8 *regs;
 231 };
 232
 233 struct ov965x_interval {
 234         struct v4l2_fract interval;
 235         /* Maximum resolution for this interval */
 236         struct v4l2_frmsize_discrete size;
 237         u8 clkrc_div;
 238 };
 239
 240 enum gpio_id {
 241         GPIO_PWDN,
 242         GPIO_RST,
 243         NUM_GPIOS,
 244 };
 245
 246 struct ov965x {
 247         struct v4l2_subdev sd;
 248         struct media_pad pad;
 249         enum v4l2_mbus_type bus_type;
 250         struct gpio_desc *gpios[NUM_GPIOS];
 251         /* External master clock frequency */
 252         unsigned long mclk_frequency;
 253         struct clk *clk;
 254
 255         /* Protects the struct fields below */
 256         struct mutex lock;
 257
 258         struct regmap *regmap;
 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 ov965x *ov965x, u8 addr, u8 *val)
 424 {
 425         int ret;
 426         unsigned int buf;
 427
 428         ret = regmap_read(ov965x->regmap, addr, &buf);
 429         if (!ret)
 430                 *val = buf;
 431         else
 432                 *val = -1;
 433
 434         v4l2_dbg(2, debug, &ov965x->sd, "%s: 0x%02x @ 0x%02x. (%d)\n",
 435                  __func__, *val, addr, ret);
 436
 437         return ret;
 438 }
 439
 440 static int ov965x_write(struct ov965x *ov965x, u8 addr, u8 val)
 441 {
 442         int ret;
 443
 444         ret = regmap_write(ov965x->regmap, addr, val);
 445
 446         v4l2_dbg(2, debug, &ov965x->sd, "%s: 0x%02x @ 0x%02X (%d)\n",
 447                  __func__, val, addr, ret);
 448
 449         return ret;
 450 }
 451
 452 static int ov965x_write_array(struct ov965x *ov965x,
 453                               const struct i2c_rv *regs)
 454 {
 455         int i, ret = 0;
 456
 457         for (i = 0; ret == 0 && regs[i].addr != REG_NULL; i++)
 458                 ret = ov965x_write(ov965x, regs[i].addr, regs[i].value);
 459
 460         return ret;
 461 }
 462
 463 static int ov965x_set_default_gamma_curve(struct ov965x *ov965x)
 464 {
 465         static const u8 gamma_curve[] = {
 466                 /* Values taken from OV application note. */
 467                 0x40, 0x30, 0x4b, 0x60, 0x70, 0x70, 0x70, 0x70,
 468                 0x60, 0x60, 0x50, 0x48, 0x3a, 0x2e, 0x28, 0x22,
 469                 0x04, 0x07, 0x10, 0x28, 0x36, 0x44, 0x52, 0x60,
 470                 0x6c, 0x78, 0x8c, 0x9e, 0xbb, 0xd2, 0xe6
 471         };
 472         u8 addr = REG_GSP;
 473         unsigned int i;
 474
 475         for (i = 0; i < ARRAY_SIZE(gamma_curve); i++) {
 476                 int ret = ov965x_write(ov965x, addr, gamma_curve[i]);
 477
 478                 if (ret < 0)
 479                         return ret;
 480                 addr++;
 481         }
 482
 483         return 0;
 484 };
 485
 486 static int ov965x_set_color_matrix(struct ov965x *ov965x)
 487 {
 488         static const u8 mtx[] = {
 489                 /* MTX1..MTX9, MTXS */
 490                 0x3a, 0x3d, 0x03, 0x12, 0x26, 0x38, 0x40, 0x40, 0x40, 0x0d
 491         };
 492         u8 addr = REG_MTX(1);
 493         unsigned int i;
 494
 495         for (i = 0; i < ARRAY_SIZE(mtx); i++) {
 496                 int ret = ov965x_write(ov965x, addr, mtx[i]);
 497
 498                 if (ret < 0)
 499                         return ret;
 500                 addr++;
 501         }
 502
 503         return 0;
 504 }
 505
 506 static int __ov965x_set_power(struct ov965x *ov965x, int on)
 507 {
 508         if (on) {
 509                 int ret = clk_prepare_enable(ov965x->clk);
 510
 511                 if (ret)
 512                         return ret;
 513
 514                 gpiod_set_value_cansleep(ov965x->gpios[GPIO_PWDN], 0);
 515                 gpiod_set_value_cansleep(ov965x->gpios[GPIO_RST], 0);
 516                 msleep(25);
 517         } else {
 518                 gpiod_set_value_cansleep(ov965x->gpios[GPIO_RST], 1);
 519                 gpiod_set_value_cansleep(ov965x->gpios[GPIO_PWDN], 1);
 520
 521                 clk_disable_unprepare(ov965x->clk);
 522         }
 523
 524         ov965x->streaming = 0;
 525
 526         return 0;
 527 }
 528
 529 static int ov965x_s_power(struct v4l2_subdev *sd, int on)
 530 {
 531         struct ov965x *ov965x = to_ov965x(sd);
 532         int ret = 0;
 533
 534         v4l2_dbg(1, debug, sd, "%s: on: %d\n", __func__, on);
 535
 536         mutex_lock(&ov965x->lock);
 537         if (ov965x->power == !on) {
 538                 ret = __ov965x_set_power(ov965x, on);
 539                 if (!ret && on) {
 540                         ret = ov965x_write_array(ov965x,
 541                                                  ov965x_init_regs);
 542                         ov965x->apply_frame_fmt = 1;
 543                         ov965x->ctrls.update = 1;
 544                 }
 545         }
 546         if (!ret)
 547                 ov965x->power += on ? 1 : -1;
 548
 549         WARN_ON(ov965x->power < 0);
 550         mutex_unlock(&ov965x->lock);
 551         return ret;
 552 }
 553
 554 /*
 555  * V4L2 controls
 556  */
 557
 558 static void ov965x_update_exposure_ctrl(struct ov965x *ov965x)
 559 {
 560         struct v4l2_ctrl *ctrl = ov965x->ctrls.exposure;
 561         unsigned long fint, trow;
 562         int min, max, def;
 563         u8 clkrc;
 564
 565         mutex_lock(&ov965x->lock);
 566         if (WARN_ON(!ctrl || !ov965x->frame_size)) {
 567                 mutex_unlock(&ov965x->lock);
 568                 return;
 569         }
 570         clkrc = DEF_CLKRC + ov965x->fiv->clkrc_div;
 571         /* Calculate internal clock frequency */
 572         fint = ov965x->mclk_frequency * ((clkrc >> 7) + 1) /
 573                                 ((2 * ((clkrc & 0x3f) + 1)));
 574         /* and the row interval (in us). */
 575         trow = (2 * 1520 * 1000000UL) / fint;
 576         max = ov965x->frame_size->max_exp_lines * trow;
 577         ov965x->exp_row_interval = trow;
 578         mutex_unlock(&ov965x->lock);
 579
 580         v4l2_dbg(1, debug, &ov965x->sd, "clkrc: %#x, fi: %lu, tr: %lu, %d\n",
 581                  clkrc, fint, trow, max);
 582
 583         /* Update exposure time range to match current frame format. */
 584         min = (trow + 100) / 100;
 585         max = (max - 100) / 100;
 586         def = min + (max - min) / 2;
 587
 588         if (v4l2_ctrl_modify_range(ctrl, min, max, 1, def))
 589                 v4l2_err(&ov965x->sd, "Exposure ctrl range update failed\n");
 590 }
 591
 592 static int ov965x_set_banding_filter(struct ov965x *ov965x, int value)
 593 {
 594         unsigned long mbd, light_freq;
 595         int ret;
 596         u8 reg;
 597
 598         ret = ov965x_read(ov965x, REG_COM8, &reg);
 599         if (!ret) {
 600                 if (value == V4L2_CID_POWER_LINE_FREQUENCY_DISABLED)
 601                         reg &= ~COM8_BFILT;
 602                 else
 603                         reg |= COM8_BFILT;
 604                 ret = ov965x_write(ov965x, REG_COM8, reg);
 605         }
 606         if (value == V4L2_CID_POWER_LINE_FREQUENCY_DISABLED)
 607                 return 0;
 608         if (WARN_ON(!ov965x->fiv))
 609                 return -EINVAL;
 610         /* Set minimal exposure time for 50/60 HZ lighting */
 611         if (value == V4L2_CID_POWER_LINE_FREQUENCY_50HZ)
 612                 light_freq = 50;
 613         else
 614                 light_freq = 60;
 615         mbd = (1000UL * ov965x->fiv->interval.denominator *
 616                ov965x->frame_size->max_exp_lines) /
 617                ov965x->fiv->interval.numerator;
 618         mbd = ((mbd / (light_freq * 2)) + 500) / 1000UL;
 619
 620         return ov965x_write(ov965x, REG_MBD, mbd);
 621 }
 622
 623 static int ov965x_set_white_balance(struct ov965x *ov965x, int awb)
 624 {
 625         int ret;
 626         u8 reg;
 627
 628         ret = ov965x_read(ov965x, REG_COM8, &reg);
 629         if (!ret) {
 630                 reg = awb ? reg | REG_COM8 : reg & ~REG_COM8;
 631                 ret = ov965x_write(ov965x, REG_COM8, reg);
 632         }
 633         if (!ret && !awb) {
 634                 ret = ov965x_write(ov965x, REG_BLUE,
 635                                    ov965x->ctrls.blue_balance->val);
 636                 if (ret < 0)
 637                         return ret;
 638                 ret = ov965x_write(ov965x, REG_RED,
 639                                    ov965x->ctrls.red_balance->val);
 640         }
 641         return ret;
 642 }
 643
 644 #define NUM_BR_LEVELS   7
 645 #define NUM_BR_REGS     3
 646
 647 static int ov965x_set_brightness(struct ov965x *ov965x, int val)
 648 {
 649         static const u8 regs[NUM_BR_LEVELS + 1][NUM_BR_REGS] = {
 650                 { REG_AEW, REG_AEB, REG_VPT },
 651                 { 0x1c, 0x12, 0x50 }, /* -3 */
 652                 { 0x3d, 0x30, 0x71 }, /* -2 */
 653                 { 0x50, 0x44, 0x92 }, /* -1 */
 654                 { 0x70, 0x64, 0xc3 }, /*  0 */
 655                 { 0x90, 0x84, 0xd4 }, /* +1 */
 656                 { 0xc4, 0xbf, 0xf9 }, /* +2 */
 657                 { 0xd8, 0xd0, 0xfa }, /* +3 */
 658         };
 659         int i, ret = 0;
 660
 661         val += (NUM_BR_LEVELS / 2 + 1);
 662         if (val > NUM_BR_LEVELS)
 663                 return -EINVAL;
 664
 665         for (i = 0; i < NUM_BR_REGS && !ret; i++)
 666                 ret = ov965x_write(ov965x, regs[0][i],
 667                                    regs[val][i]);
 668         return ret;
 669 }
 670
 671 static int ov965x_set_gain(struct ov965x *ov965x, int auto_gain)
 672 {
 673         struct ov965x_ctrls *ctrls = &ov965x->ctrls;
 674         int ret = 0;
 675         u8 reg;
 676         /*
 677          * For manual mode we need to disable AGC first, so
 678          * gain value in REG_VREF, REG_GAIN is not overwritten.
 679          */
 680         if (ctrls->auto_gain->is_new) {
 681                 ret = ov965x_read(ov965x, REG_COM8, &reg);
 682                 if (ret < 0)
 683                         return ret;
 684                 if (ctrls->auto_gain->val)
 685                         reg |= COM8_AGC;
 686                 else
 687                         reg &= ~COM8_AGC;
 688                 ret = ov965x_write(ov965x, REG_COM8, reg);
 689                 if (ret < 0)
 690                         return ret;
 691         }
 692
 693         if (ctrls->gain->is_new && !auto_gain) {
 694                 unsigned int gain = ctrls->gain->val;
 695                 unsigned int rgain;
 696                 int m;
 697                 /*
 698                  * Convert gain control value to the sensor's gain
 699                  * registers (VREF[7:6], GAIN[7:0]) format.
 700                  */
 701                 for (m = 6; m >= 0; m--)
 702                         if (gain >= (1 << m) * 16)
 703                                 break;
 704
 705                 /* Sanity check: don't adjust the gain with a negative value */
 706                 if (m < 0)
 707                         return -EINVAL;
 708
 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_state *sd_state,
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_state *sd_state,
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_state *sd_state,
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, sd_state, 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_state *sd_state,
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 (sd_state) {
1236                         mf = v4l2_subdev_get_try_format(sd, sd_state,
1237                                                         fmt->pad);
1238                         *mf = fmt->format;
1239                 }
1240         } else {
1241                 if (ov965x->streaming) {
1242                         ret = -EBUSY;
1243                 } else {
1244                         ov965x->frame_size = size;
1245                         ov965x->format = fmt->format;
1246                         ov965x->tslb_reg = ov965x_formats[index].tslb_reg;
1247                         ov965x->apply_frame_fmt = 1;
1248                 }
1249         }
1250
1251         if (!ret && fmt->which == V4L2_SUBDEV_FORMAT_ACTIVE) {
1252                 struct v4l2_subdev_frame_interval fiv = {
1253                         .interval = { 0, 1 }
1254                 };
1255                 /* Reset to minimum possible frame interval */
1256                 __ov965x_set_frame_interval(ov965x, &fiv);
1257         }
1258         mutex_unlock(&ov965x->lock);
1259
1260         if (!ret)
1261                 ov965x_update_exposure_ctrl(ov965x);
1262
1263         return ret;
1264 }
1265
1266 static int ov965x_set_frame_size(struct ov965x *ov965x)
1267 {
1268         int i, ret = 0;
1269
1270         for (i = 0; ret == 0 && i < NUM_FMT_REGS; i++)
1271                 ret = ov965x_write(ov965x, frame_size_reg_addr[i],
1272                                    ov965x->frame_size->regs[i]);
1273         return ret;
1274 }
1275
1276 static int __ov965x_set_params(struct ov965x *ov965x)
1277 {
1278         struct ov965x_ctrls *ctrls = &ov965x->ctrls;
1279         int ret = 0;
1280         u8 reg;
1281
1282         if (ov965x->apply_frame_fmt) {
1283                 reg = DEF_CLKRC + ov965x->fiv->clkrc_div;
1284                 ret = ov965x_write(ov965x, REG_CLKRC, reg);
1285                 if (ret < 0)
1286                         return ret;
1287                 ret = ov965x_set_frame_size(ov965x);
1288                 if (ret < 0)
1289                         return ret;
1290                 ret = ov965x_read(ov965x, REG_TSLB, &reg);
1291                 if (ret < 0)
1292                         return ret;
1293                 reg &= ~TSLB_YUYV_MASK;
1294                 reg |= ov965x->tslb_reg;
1295                 ret = ov965x_write(ov965x, REG_TSLB, reg);
1296                 if (ret < 0)
1297                         return ret;
1298         }
1299         ret = ov965x_set_default_gamma_curve(ov965x);
1300         if (ret < 0)
1301                 return ret;
1302         ret = ov965x_set_color_matrix(ov965x);
1303         if (ret < 0)
1304                 return ret;
1305         /*
1306          * Select manual banding filter, the filter will
1307          * be enabled further if required.
1308          */
1309         ret = ov965x_read(ov965x, REG_COM11, &reg);
1310         if (!ret)
1311                 reg |= COM11_BANDING;
1312         ret = ov965x_write(ov965x, REG_COM11, reg);
1313         if (ret < 0)
1314                 return ret;
1315         /*
1316          * Banding filter (REG_MBD value) needs to match selected
1317          * resolution and frame rate, so it's always updated here.
1318          */
1319         return ov965x_set_banding_filter(ov965x, ctrls->light_freq->val);
1320 }
1321
1322 static int ov965x_s_stream(struct v4l2_subdev *sd, int on)
1323 {
1324         struct ov965x *ov965x = to_ov965x(sd);
1325         struct ov965x_ctrls *ctrls = &ov965x->ctrls;
1326         int ret = 0;
1327
1328         v4l2_dbg(1, debug, sd, "%s: on: %d\n", __func__, on);
1329
1330         mutex_lock(&ov965x->lock);
1331         if (ov965x->streaming == !on) {
1332                 if (on)
1333                         ret = __ov965x_set_params(ov965x);
1334
1335                 if (!ret && ctrls->update) {
1336                         /*
1337                          * ov965x_s_ctrl callback takes the mutex
1338                          * so it needs to be released here.
1339                          */
1340                         mutex_unlock(&ov965x->lock);
1341                         ret = v4l2_ctrl_handler_setup(&ctrls->handler);
1342
1343                         mutex_lock(&ov965x->lock);
1344                         if (!ret)
1345                                 ctrls->update = 0;
1346                 }
1347                 if (!ret)
1348                         ret = ov965x_write(ov965x, REG_COM2,
1349                                            on ? 0x01 : 0x11);
1350         }
1351         if (!ret)
1352                 ov965x->streaming += on ? 1 : -1;
1353
1354         WARN_ON(ov965x->streaming < 0);
1355         mutex_unlock(&ov965x->lock);
1356
1357         return ret;
1358 }
1359
1360 /*
1361  * V4L2 subdev internal operations
1362  */
1363 static int ov965x_open(struct v4l2_subdev *sd, struct v4l2_subdev_fh *fh)
1364 {
1365         struct v4l2_mbus_framefmt *mf =
1366                 v4l2_subdev_get_try_format(sd, fh->state, 0);
1367
1368         ov965x_get_default_format(mf);
1369         return 0;
1370 }
1371
1372 static const struct v4l2_subdev_pad_ops ov965x_pad_ops = {
1373         .enum_mbus_code = ov965x_enum_mbus_code,
1374         .enum_frame_size = ov965x_enum_frame_sizes,
1375         .get_fmt = ov965x_get_fmt,
1376         .set_fmt = ov965x_set_fmt,
1377 };
1378
1379 static const struct v4l2_subdev_video_ops ov965x_video_ops = {
1380         .s_stream = ov965x_s_stream,
1381         .g_frame_interval = ov965x_g_frame_interval,
1382         .s_frame_interval = ov965x_s_frame_interval,
1383
1384 };
1385
1386 static const struct v4l2_subdev_internal_ops ov965x_sd_internal_ops = {
1387         .open = ov965x_open,
1388 };
1389
1390 static const struct v4l2_subdev_core_ops ov965x_core_ops = {
1391         .s_power = ov965x_s_power,
1392         .log_status = v4l2_ctrl_subdev_log_status,
1393         .subscribe_event = v4l2_ctrl_subdev_subscribe_event,
1394         .unsubscribe_event = v4l2_event_subdev_unsubscribe,
1395 };
1396
1397 static const struct v4l2_subdev_ops ov965x_subdev_ops = {
1398         .core = &ov965x_core_ops,
1399         .pad = &ov965x_pad_ops,
1400         .video = &ov965x_video_ops,
1401 };
1402
1403 static int ov965x_configure_gpios(struct ov965x *ov965x)
1404 {
1405         struct device *dev = regmap_get_device(ov965x->regmap);
1406
1407         ov965x->gpios[GPIO_PWDN] = devm_gpiod_get_optional(dev, "powerdown",
1408                                                         GPIOD_OUT_HIGH);
1409         if (IS_ERR(ov965x->gpios[GPIO_PWDN])) {
1410                 dev_info(dev, "can't get %s GPIO\n", "powerdown");
1411                 return PTR_ERR(ov965x->gpios[GPIO_PWDN]);
1412         }
1413
1414         ov965x->gpios[GPIO_RST] = devm_gpiod_get_optional(dev, "reset",
1415                                                         GPIOD_OUT_HIGH);
1416         if (IS_ERR(ov965x->gpios[GPIO_RST])) {
1417                 dev_info(dev, "can't get %s GPIO\n", "reset");
1418                 return PTR_ERR(ov965x->gpios[GPIO_RST]);
1419         }
1420
1421         return 0;
1422 }
1423
1424 static int ov965x_detect_sensor(struct v4l2_subdev *sd)
1425 {
1426         struct ov965x *ov965x = to_ov965x(sd);
1427         u8 pid, ver;
1428         int ret;
1429
1430         mutex_lock(&ov965x->lock);
1431         ret = __ov965x_set_power(ov965x, 1);
1432         if (ret)
1433                 goto out;
1434
1435         msleep(25);
1436
1437         /* Check sensor revision */
1438         ret = ov965x_read(ov965x, REG_PID, &pid);
1439         if (!ret)
1440                 ret = ov965x_read(ov965x, REG_VER, &ver);
1441
1442         __ov965x_set_power(ov965x, 0);
1443
1444         if (!ret) {
1445                 ov965x->id = OV965X_ID(pid, ver);
1446                 if (ov965x->id == OV9650_ID || ov965x->id == OV9652_ID) {
1447                         v4l2_info(sd, "Found OV%04X sensor\n", ov965x->id);
1448                 } else {
1449                         v4l2_err(sd, "Sensor detection failed (%04X)\n",
1450                                  ov965x->id);
1451                         ret = -ENODEV;
1452                 }
1453         }
1454 out:
1455         mutex_unlock(&ov965x->lock);
1456
1457         return ret;
1458 }
1459
1460 static int ov965x_probe(struct i2c_client *client)
1461 {
1462         struct v4l2_subdev *sd;
1463         struct ov965x *ov965x;
1464         int ret;
1465         static const struct regmap_config ov965x_regmap_config = {
1466                 .reg_bits = 8,
1467                 .val_bits = 8,
1468                 .max_register = 0xab,
1469         };
1470
1471         ov965x = devm_kzalloc(&client->dev, sizeof(*ov965x), GFP_KERNEL);
1472         if (!ov965x)
1473                 return -ENOMEM;
1474
1475         ov965x->regmap = devm_regmap_init_sccb(client, &ov965x_regmap_config);
1476         if (IS_ERR(ov965x->regmap)) {
1477                 dev_err(&client->dev, "Failed to allocate register map\n");
1478                 return PTR_ERR(ov965x->regmap);
1479         }
1480
1481         if (dev_fwnode(&client->dev)) {
1482                 ov965x->clk = devm_clk_get(&client->dev, NULL);
1483                 if (IS_ERR(ov965x->clk))
1484                         return PTR_ERR(ov965x->clk);
1485                 ov965x->mclk_frequency = clk_get_rate(ov965x->clk);
1486
1487                 ret = ov965x_configure_gpios(ov965x);
1488                 if (ret < 0)
1489                         return ret;
1490         } else {
1491                 dev_err(&client->dev,
1492                         "No device properties specified\n");
1493
1494                 return -EINVAL;
1495         }
1496
1497         mutex_init(&ov965x->lock);
1498
1499         sd = &ov965x->sd;
1500         v4l2_i2c_subdev_init(sd, client, &ov965x_subdev_ops);
1501         strscpy(sd->name, DRIVER_NAME, sizeof(sd->name));
1502
1503         sd->internal_ops = &ov965x_sd_internal_ops;
1504         sd->flags |= V4L2_SUBDEV_FL_HAS_DEVNODE |
1505                      V4L2_SUBDEV_FL_HAS_EVENTS;
1506
1507         ov965x->pad.flags = MEDIA_PAD_FL_SOURCE;
1508         sd->entity.function = MEDIA_ENT_F_CAM_SENSOR;
1509         ret = media_entity_pads_init(&sd->entity, 1, &ov965x->pad);
1510         if (ret < 0)
1511                 goto err_mutex;
1512
1513         ret = ov965x_initialize_controls(ov965x);
1514         if (ret < 0)
1515                 goto err_me;
1516
1517         ov965x_get_default_format(&ov965x->format);
1518         ov965x->frame_size = &ov965x_framesizes[0];
1519         ov965x->fiv = &ov965x_intervals[0];
1520
1521         ret = ov965x_detect_sensor(sd);
1522         if (ret < 0)
1523                 goto err_ctrls;
1524
1525         /* Update exposure time min/max to match frame format */
1526         ov965x_update_exposure_ctrl(ov965x);
1527
1528         ret = v4l2_async_register_subdev(sd);
1529         if (ret < 0)
1530                 goto err_ctrls;
1531
1532         return 0;
1533 err_ctrls:
1534         v4l2_ctrl_handler_free(sd->ctrl_handler);
1535 err_me:
1536         media_entity_cleanup(&sd->entity);
1537 err_mutex:
1538         mutex_destroy(&ov965x->lock);
1539         return ret;
1540 }
1541
1542 static void ov965x_remove(struct i2c_client *client)
1543 {
1544         struct v4l2_subdev *sd = i2c_get_clientdata(client);
1545         struct ov965x *ov965x = to_ov965x(sd);
1546
1547         v4l2_async_unregister_subdev(sd);
1548         v4l2_ctrl_handler_free(sd->ctrl_handler);
1549         media_entity_cleanup(&sd->entity);
1550         mutex_destroy(&ov965x->lock);
1551 }
1552
1553 static const struct i2c_device_id ov965x_id[] = {
1554         { "OV9650", 0 },
1555         { "OV9652", 0 },
1556         { /* sentinel */ }
1557 };
1558 MODULE_DEVICE_TABLE(i2c, ov965x_id);
1559
1560 #if IS_ENABLED(CONFIG_OF)
1561 static const struct of_device_id ov965x_of_match[] = {
1562         { .compatible = "ovti,ov9650", },
1563         { .compatible = "ovti,ov9652", },
1564         { /* sentinel */ }
1565 };
1566 MODULE_DEVICE_TABLE(of, ov965x_of_match);
1567 #endif
1568
1569 static struct i2c_driver ov965x_i2c_driver = {
1570         .driver = {
1571                 .name   = DRIVER_NAME,
1572                 .of_match_table = of_match_ptr(ov965x_of_match),
1573         },
1574         .probe          = ov965x_probe,
1575         .remove         = ov965x_remove,
1576         .id_table       = ov965x_id,
1577 };
1578
1579 module_i2c_driver(ov965x_i2c_driver);
1580
1581 MODULE_AUTHOR("Sylwester Nawrocki <sylvester.nawrocki@gmail.com>");
1582 MODULE_DESCRIPTION("OV9650/OV9652 CMOS Image Sensor driver");
1583 MODULE_LICENSE("GPL");