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