drivers/media/video/s5k6aa.c

   1 /*
   2  * Driver for Samsung S5K6AAFX SXGA 1/6" 1.3M CMOS Image Sensor
   3  * with embedded SoC ISP.
   4  *
   5  * Copyright (C) 2011, Samsung Electronics Co., Ltd.
   6  * Sylwester Nawrocki <s.nawrocki@samsung.com>
   7  *
   8  * Based on a driver authored by Dongsoo Nathaniel Kim.
   9  * Copyright (C) 2009, Dongsoo Nathaniel Kim <dongsoo45.kim@samsung.com>
  10  *
  11  * This program is free software; you can redistribute it and/or modify
  12  * it under the terms of the GNU General Public License as published by
  13  * the Free Software Foundation; either version 2 of the License, or
  14  * (at your option) any later version.
  15  */
  16
  17 #include <linux/clk.h>
  18 #include <linux/delay.h>
  19 #include <linux/gpio.h>
  20 #include <linux/i2c.h>
  21 #include <linux/media.h>
  22 #include <linux/regulator/consumer.h>
  23 #include <linux/slab.h>
  24
  25 #include <media/media-entity.h>
  26 #include <media/v4l2-ctrls.h>
  27 #include <media/v4l2-device.h>
  28 #include <media/v4l2-subdev.h>
  29 #include <media/v4l2-mediabus.h>
  30 #include <media/s5k6aa.h>
  31
  32 static int debug;
  33 module_param(debug, int, 0644);
  34
  35 #define DRIVER_NAME                     "S5K6AA"
  36
  37 /* The token to indicate array termination */
  38 #define S5K6AA_TERM                     0xffff
  39 #define S5K6AA_OUT_WIDTH_DEF            640
  40 #define S5K6AA_OUT_HEIGHT_DEF           480
  41 #define S5K6AA_WIN_WIDTH_MAX            1280
  42 #define S5K6AA_WIN_HEIGHT_MAX           1024
  43 #define S5K6AA_WIN_WIDTH_MIN            8
  44 #define S5K6AA_WIN_HEIGHT_MIN           8
  45
  46 /*
  47  * H/W register Interface (0xD0000000 - 0xD0000FFF)
  48  */
  49 #define AHB_MSB_ADDR_PTR                0xfcfc
  50 #define GEN_REG_OFFSH                   0xd000
  51 #define REG_CMDWR_ADDRH                 0x0028
  52 #define REG_CMDWR_ADDRL                 0x002a
  53 #define REG_CMDRD_ADDRH                 0x002c
  54 #define REG_CMDRD_ADDRL                 0x002e
  55 #define REG_CMDBUF0_ADDR                0x0f12
  56 #define REG_CMDBUF1_ADDR                0x0f10
  57
  58 /*
  59  * Host S/W Register interface (0x70000000 - 0x70002000)
  60  * The value of the two most significant address bytes is 0x7000,
  61  * (HOST_SWIF_OFFS_H). The register addresses below specify 2 LSBs.
  62  */
  63 #define HOST_SWIF_OFFSH                 0x7000
  64
  65 /* Initialization parameters */
  66 /* Master clock frequency in KHz */
  67 #define REG_I_INCLK_FREQ_L              0x01b8
  68 #define REG_I_INCLK_FREQ_H              0x01ba
  69 #define  MIN_MCLK_FREQ_KHZ              6000U
  70 #define  MAX_MCLK_FREQ_KHZ              27000U
  71 #define REG_I_USE_NPVI_CLOCKS           0x01c6
  72 #define REG_I_USE_NMIPI_CLOCKS          0x01c8
  73
  74 /* Clock configurations, n = 0..2. REG_I_* frequency unit is 4 kHz. */
  75 #define REG_I_OPCLK_4KHZ(n)             ((n) * 6 + 0x01cc)
  76 #define REG_I_MIN_OUTRATE_4KHZ(n)       ((n) * 6 + 0x01ce)
  77 #define REG_I_MAX_OUTRATE_4KHZ(n)       ((n) * 6 + 0x01d0)
  78 #define  SYS_PLL_OUT_FREQ               (48000000 / 4000)
  79 #define  PCLK_FREQ_MIN                  (24000000 / 4000)
  80 #define  PCLK_FREQ_MAX                  (48000000 / 4000)
  81 #define REG_I_INIT_PARAMS_UPDATED       0x01e0
  82 #define REG_I_ERROR_INFO                0x01e2
  83
  84 /* General purpose parameters */
  85 #define REG_USER_BRIGHTNESS             0x01e4
  86 #define REG_USER_CONTRAST               0x01e6
  87 #define REG_USER_SATURATION             0x01e8
  88 #define REG_USER_SHARPBLUR              0x01ea
  89
  90 #define REG_G_SPEC_EFFECTS              0x01ee
  91 #define REG_G_ENABLE_PREV               0x01f0
  92 #define REG_G_ENABLE_PREV_CHG           0x01f2
  93 #define REG_G_NEW_CFG_SYNC              0x01f8
  94 #define REG_G_PREVZOOM_IN_WIDTH         0x020a
  95 #define REG_G_PREVZOOM_IN_HEIGHT        0x020c
  96 #define REG_G_PREVZOOM_IN_XOFFS         0x020e
  97 #define REG_G_PREVZOOM_IN_YOFFS         0x0210
  98 #define REG_G_INPUTS_CHANGE_REQ         0x021a
  99 #define REG_G_ACTIVE_PREV_CFG           0x021c
 100 #define REG_G_PREV_CFG_CHG              0x021e
 101 #define REG_G_PREV_OPEN_AFTER_CH        0x0220
 102 #define REG_G_PREV_CFG_ERROR            0x0222
 103
 104 /* Preview control section. n = 0...4. */
 105 #define PREG(n, x)                      ((n) * 0x26 + x)
 106 #define REG_P_OUT_WIDTH(n)              PREG(n, 0x0242)
 107 #define REG_P_OUT_HEIGHT(n)             PREG(n, 0x0244)
 108 #define REG_P_FMT(n)                    PREG(n, 0x0246)
 109 #define REG_P_MAX_OUT_RATE(n)           PREG(n, 0x0248)
 110 #define REG_P_MIN_OUT_RATE(n)           PREG(n, 0x024a)
 111 #define REG_P_PVI_MASK(n)               PREG(n, 0x024c)
 112 #define REG_P_CLK_INDEX(n)              PREG(n, 0x024e)
 113 #define REG_P_FR_RATE_TYPE(n)           PREG(n, 0x0250)
 114 #define  FR_RATE_DYNAMIC                0
 115 #define  FR_RATE_FIXED                  1
 116 #define  FR_RATE_FIXED_ACCURATE         2
 117 #define REG_P_FR_RATE_Q_TYPE(n)         PREG(n, 0x0252)
 118 #define  FR_RATE_Q_BEST_FRRATE          1 /* Binning enabled */
 119 #define  FR_RATE_Q_BEST_QUALITY         2 /* Binning disabled */
 120 /* Frame period in 0.1 ms units */
 121 #define REG_P_MAX_FR_TIME(n)            PREG(n, 0x0254)
 122 #define REG_P_MIN_FR_TIME(n)            PREG(n, 0x0256)
 123 /* Conversion to REG_P_[MAX/MIN]_FR_TIME value; __t: time in us */
 124 #define  US_TO_FR_TIME(__t)             ((__t) / 100)
 125 #define  S5K6AA_MIN_FR_TIME             33300  /* us */
 126 #define  S5K6AA_MAX_FR_TIME             650000 /* us */
 127 #define  S5K6AA_MAX_HIGHRES_FR_TIME     666    /* x100 us */
 128 /* The below 5 registers are for "device correction" values */
 129 #define REG_P_COLORTEMP(n)              PREG(n, 0x025e)
 130 #define REG_P_PREV_MIRROR(n)            PREG(n, 0x0262)
 131
 132 /* Extended image property controls */
 133 /* Exposure time in 10 us units */
 134 #define REG_SF_USR_EXPOSURE_L           0x03c6
 135 #define REG_SF_USR_EXPOSURE_H           0x03c8
 136 #define REG_SF_USR_EXPOSURE_CHG         0x03ca
 137 #define REG_SF_USR_TOT_GAIN             0x03cc
 138 #define REG_SF_USR_TOT_GAIN_CHG         0x03ce
 139 #define REG_SF_RGAIN                    0x03d0
 140 #define REG_SF_RGAIN_CHG                0x03d2
 141 #define REG_SF_GGAIN                    0x03d4
 142 #define REG_SF_GGAIN_CHG                0x03d6
 143 #define REG_SF_BGAIN                    0x03d8
 144 #define REG_SF_BGAIN_CHG                0x03da
 145 #define REG_SF_FLICKER_QUANT            0x03dc
 146 #define REG_SF_FLICKER_QUANT_CHG        0x03de
 147
 148 /* Output interface (parallel/MIPI) setup */
 149 #define REG_OIF_EN_MIPI_LANES           0x03fa
 150 #define REG_OIF_EN_PACKETS              0x03fc
 151 #define REG_OIF_CFG_CHG                 0x03fe
 152
 153 /* Auto-algorithms enable mask */
 154 #define REG_DBG_AUTOALG_EN              0x0400
 155 #define  AALG_ALL_EN_MASK               (1 << 0)
 156 #define  AALG_AE_EN_MASK                (1 << 1)
 157 #define  AALG_DIVLEI_EN_MASK            (1 << 2)
 158 #define  AALG_WB_EN_MASK                (1 << 3)
 159 #define  AALG_FLICKER_EN_MASK           (1 << 5)
 160 #define  AALG_FIT_EN_MASK               (1 << 6)
 161 #define  AALG_WRHW_EN_MASK              (1 << 7)
 162
 163 /* Firmware revision information */
 164 #define REG_FW_APIVER                   0x012e
 165 #define  S5K6AAFX_FW_APIVER             0x0001
 166 #define REG_FW_REVISION                 0x0130
 167
 168 /* For now we use only one user configuration register set */
 169 #define S5K6AA_MAX_PRESETS              1
 170
 171 static const char * const s5k6aa_supply_names[] = {
 172         "vdd_core",     /* Digital core supply 1.5V (1.4V to 1.6V) */
 173         "vdda",         /* Analog power supply 2.8V (2.6V to 3.0V) */
 174         "vdd_reg",      /* Regulator input power 1.8V (1.7V to 1.9V)
 175                            or 2.8V (2.6V to 3.0) */
 176         "vddio",        /* I/O supply 1.8V (1.65V to 1.95V)
 177                            or 2.8V (2.5V to 3.1V) */
 178 };
 179 #define S5K6AA_NUM_SUPPLIES ARRAY_SIZE(s5k6aa_supply_names)
 180
 181 enum s5k6aa_gpio_id {
 182         STBY,
 183         RST,
 184         GPIO_NUM,
 185 };
 186
 187 struct s5k6aa_regval {
 188         u16 addr;
 189         u16 val;
 190 };
 191
 192 struct s5k6aa_pixfmt {
 193         enum v4l2_mbus_pixelcode code;
 194         u32 colorspace;
 195         /* REG_P_FMT(x) register value */
 196         u16 reg_p_fmt;
 197 };
 198
 199 struct s5k6aa_preset {
 200         /* output pixel format and resolution */
 201         struct v4l2_mbus_framefmt mbus_fmt;
 202         u8 clk_id;
 203         u8 index;
 204 };
 205
 206 struct s5k6aa_ctrls {
 207         struct v4l2_ctrl_handler handler;
 208         /* Auto / manual white balance cluster */
 209         struct v4l2_ctrl *awb;
 210         struct v4l2_ctrl *gain_red;
 211         struct v4l2_ctrl *gain_blue;
 212         struct v4l2_ctrl *gain_green;
 213         /* Mirror cluster */
 214         struct v4l2_ctrl *hflip;
 215         struct v4l2_ctrl *vflip;
 216         /* Auto exposure / manual exposure and gain cluster */
 217         struct v4l2_ctrl *auto_exp;
 218         struct v4l2_ctrl *exposure;
 219         struct v4l2_ctrl *gain;
 220 };
 221
 222 struct s5k6aa_interval {
 223         u16 reg_fr_time;
 224         struct v4l2_fract interval;
 225         /* Maximum rectangle for the interval */
 226         struct v4l2_frmsize_discrete size;
 227 };
 228
 229 struct s5k6aa {
 230         struct v4l2_subdev sd;
 231         struct media_pad pad;
 232
 233         enum v4l2_mbus_type bus_type;
 234         u8 mipi_lanes;
 235
 236         int (*s_power)(int enable);
 237         struct regulator_bulk_data supplies[S5K6AA_NUM_SUPPLIES];
 238         struct s5k6aa_gpio gpio[GPIO_NUM];
 239
 240         /* external master clock frequency */
 241         unsigned long mclk_frequency;
 242         /* ISP internal master clock frequency */
 243         u16 clk_fop;
 244         /* output pixel clock frequency range */
 245         u16 pclk_fmin;
 246         u16 pclk_fmax;
 247
 248         unsigned int inv_hflip:1;
 249         unsigned int inv_vflip:1;
 250
 251         /* protects the struct members below */
 252         struct mutex lock;
 253
 254         /* sensor matrix scan window */
 255         struct v4l2_rect ccd_rect;
 256
 257         struct s5k6aa_ctrls ctrls;
 258         struct s5k6aa_preset presets[S5K6AA_MAX_PRESETS];
 259         struct s5k6aa_preset *preset;
 260         const struct s5k6aa_interval *fiv;
 261
 262         unsigned int streaming:1;
 263         unsigned int apply_cfg:1;
 264         unsigned int apply_crop:1;
 265         unsigned int power;
 266 };
 267
 268 static struct s5k6aa_regval s5k6aa_analog_config[] = {
 269         /* Analog settings */
 270         { 0x112a, 0x0000 }, { 0x1132, 0x0000 },
 271         { 0x113e, 0x0000 }, { 0x115c, 0x0000 },
 272         { 0x1164, 0x0000 }, { 0x1174, 0x0000 },
 273         { 0x1178, 0x0000 }, { 0x077a, 0x0000 },
 274         { 0x077c, 0x0000 }, { 0x077e, 0x0000 },
 275         { 0x0780, 0x0000 }, { 0x0782, 0x0000 },
 276         { 0x0784, 0x0000 }, { 0x0786, 0x0000 },
 277         { 0x0788, 0x0000 }, { 0x07a2, 0x0000 },
 278         { 0x07a4, 0x0000 }, { 0x07a6, 0x0000 },
 279         { 0x07a8, 0x0000 }, { 0x07b6, 0x0000 },
 280         { 0x07b8, 0x0002 }, { 0x07ba, 0x0004 },
 281         { 0x07bc, 0x0004 }, { 0x07be, 0x0005 },
 282         { 0x07c0, 0x0005 }, { S5K6AA_TERM, 0 },
 283 };
 284
 285 /* TODO: Add RGB888 and Bayer format */
 286 static const struct s5k6aa_pixfmt s5k6aa_formats[] = {
 287         { V4L2_MBUS_FMT_YUYV8_2X8,      V4L2_COLORSPACE_JPEG,   5 },
 288         /* range 16-240 */
 289         { V4L2_MBUS_FMT_YUYV8_2X8,      V4L2_COLORSPACE_REC709, 6 },
 290         { V4L2_MBUS_FMT_RGB565_2X8_BE,  V4L2_COLORSPACE_JPEG,   0 },
 291 };
 292
 293 static const struct s5k6aa_interval s5k6aa_intervals[] = {
 294         { 1000, {10000, 1000000}, {1280, 1024} }, /* 10 fps */
 295         { 666,  {15000, 1000000}, {1280, 1024} }, /* 15 fps */
 296         { 500,  {20000, 1000000}, {1280, 720} },  /* 20 fps */
 297         { 400,  {25000, 1000000}, {640, 480} },   /* 25 fps */
 298         { 333,  {33300, 1000000}, {640, 480} },   /* 30 fps */
 299 };
 300
 301 #define S5K6AA_INTERVAL_DEF_INDEX 1 /* 15 fps */
 302
 303 static inline struct v4l2_subdev *ctrl_to_sd(struct v4l2_ctrl *ctrl)
 304 {
 305         return &container_of(ctrl->handler, struct s5k6aa, ctrls.handler)->sd;
 306 }
 307
 308 static inline struct s5k6aa *to_s5k6aa(struct v4l2_subdev *sd)
 309 {
 310         return container_of(sd, struct s5k6aa, sd);
 311 }
 312
 313 /* Set initial values for all preview presets */
 314 static void s5k6aa_presets_data_init(struct s5k6aa *s5k6aa)
 315 {
 316         struct s5k6aa_preset *preset = &s5k6aa->presets[0];
 317         int i;
 318
 319         for (i = 0; i < S5K6AA_MAX_PRESETS; i++) {
 320                 preset->mbus_fmt.width  = S5K6AA_OUT_WIDTH_DEF;
 321                 preset->mbus_fmt.height = S5K6AA_OUT_HEIGHT_DEF;
 322                 preset->mbus_fmt.code   = s5k6aa_formats[0].code;
 323                 preset->index           = i;
 324                 preset->clk_id          = 0;
 325                 preset++;
 326         }
 327
 328         s5k6aa->fiv = &s5k6aa_intervals[S5K6AA_INTERVAL_DEF_INDEX];
 329         s5k6aa->preset = &s5k6aa->presets[0];
 330 }
 331
 332 static int s5k6aa_i2c_read(struct i2c_client *client, u16 addr, u16 *val)
 333 {
 334         u8 wbuf[2] = {addr >> 8, addr & 0xFF};
 335         struct i2c_msg msg[2];
 336         u8 rbuf[2];
 337         int ret;
 338
 339         msg[0].addr = client->addr;
 340         msg[0].flags = 0;
 341         msg[0].len = 2;
 342         msg[0].buf = wbuf;
 343
 344         msg[1].addr = client->addr;
 345         msg[1].flags = I2C_M_RD;
 346         msg[1].len = 2;
 347         msg[1].buf = rbuf;
 348
 349         ret = i2c_transfer(client->adapter, msg, 2);
 350         *val = be16_to_cpu(*((u16 *)rbuf));
 351
 352         v4l2_dbg(3, debug, client, "i2c_read: 0x%04X : 0x%04x\n", addr, *val);
 353
 354         return ret == 2 ? 0 : ret;
 355 }
 356
 357 static int s5k6aa_i2c_write(struct i2c_client *client, u16 addr, u16 val)
 358 {
 359         u8 buf[4] = {addr >> 8, addr & 0xFF, val >> 8, val & 0xFF};
 360
 361         int ret = i2c_master_send(client, buf, 4);
 362         v4l2_dbg(3, debug, client, "i2c_write: 0x%04X : 0x%04x\n", addr, val);
 363
 364         return ret == 4 ? 0 : ret;
 365 }
 366
 367 /* The command register write, assumes Command_Wr_addH = 0x7000. */
 368 static int s5k6aa_write(struct i2c_client *c, u16 addr, u16 val)
 369 {
 370         int ret = s5k6aa_i2c_write(c, REG_CMDWR_ADDRL, addr);
 371         if (ret)
 372                 return ret;
 373         return s5k6aa_i2c_write(c, REG_CMDBUF0_ADDR, val);
 374 }
 375
 376 /* The command register read, assumes Command_Rd_addH = 0x7000. */
 377 static int s5k6aa_read(struct i2c_client *client, u16 addr, u16 *val)
 378 {
 379         int ret = s5k6aa_i2c_write(client, REG_CMDRD_ADDRL, addr);
 380         if (ret)
 381                 return ret;
 382         return s5k6aa_i2c_read(client, REG_CMDBUF0_ADDR, val);
 383 }
 384
 385 static int s5k6aa_write_array(struct v4l2_subdev *sd,
 386                               const struct s5k6aa_regval *msg)
 387 {
 388         struct i2c_client *client = v4l2_get_subdevdata(sd);
 389         u16 addr_incr = 0;
 390         int ret = 0;
 391
 392         while (msg->addr != S5K6AA_TERM) {
 393                 if (addr_incr != 2)
 394                         ret = s5k6aa_i2c_write(client, REG_CMDWR_ADDRL,
 395                                                msg->addr);
 396                 if (ret)
 397                         break;
 398                 ret = s5k6aa_i2c_write(client, REG_CMDBUF0_ADDR, msg->val);
 399                 if (ret)
 400                         break;
 401                 /* Assume that msg->addr is always less than 0xfffc */
 402                 addr_incr = (msg + 1)->addr - msg->addr;
 403                 msg++;
 404         }
 405
 406         return ret;
 407 }
 408
 409 /* Configure the AHB high address bytes for GTG registers access */
 410 static int s5k6aa_set_ahb_address(struct i2c_client *client)
 411 {
 412         int ret = s5k6aa_i2c_write(client, AHB_MSB_ADDR_PTR, GEN_REG_OFFSH);
 413         if (ret)
 414                 return ret;
 415         ret = s5k6aa_i2c_write(client, REG_CMDRD_ADDRH, HOST_SWIF_OFFSH);
 416         if (ret)
 417                 return ret;
 418         return s5k6aa_i2c_write(client, REG_CMDWR_ADDRH, HOST_SWIF_OFFSH);
 419 }
 420
 421 /**
 422  * s5k6aa_configure_pixel_clock - apply ISP main clock/PLL configuration
 423  *
 424  * Configure the internal ISP PLL for the required output frequency.
 425  * Locking: called with s5k6aa.lock mutex held.
 426  */
 427 static int s5k6aa_configure_pixel_clocks(struct s5k6aa *s5k6aa)
 428 {
 429         struct i2c_client *c = v4l2_get_subdevdata(&s5k6aa->sd);
 430         unsigned long fmclk = s5k6aa->mclk_frequency / 1000;
 431         u16 status;
 432         int ret;
 433
 434         if (WARN(fmclk < MIN_MCLK_FREQ_KHZ || fmclk > MAX_MCLK_FREQ_KHZ,
 435                  "Invalid clock frequency: %ld\n", fmclk))
 436                 return -EINVAL;
 437
 438         s5k6aa->pclk_fmin = PCLK_FREQ_MIN;
 439         s5k6aa->pclk_fmax = PCLK_FREQ_MAX;
 440         s5k6aa->clk_fop = SYS_PLL_OUT_FREQ;
 441
 442         /* External input clock frequency in kHz */
 443         ret = s5k6aa_write(c, REG_I_INCLK_FREQ_H, fmclk >> 16);
 444         if (!ret)
 445                 ret = s5k6aa_write(c, REG_I_INCLK_FREQ_L, fmclk & 0xFFFF);
 446         if (!ret)
 447                 ret = s5k6aa_write(c, REG_I_USE_NPVI_CLOCKS, 1);
 448         /* Internal PLL frequency */
 449         if (!ret)
 450                 ret = s5k6aa_write(c, REG_I_OPCLK_4KHZ(0), s5k6aa->clk_fop);
 451         if (!ret)
 452                 ret = s5k6aa_write(c, REG_I_MIN_OUTRATE_4KHZ(0),
 453                                    s5k6aa->pclk_fmin);
 454         if (!ret)
 455                 ret = s5k6aa_write(c, REG_I_MAX_OUTRATE_4KHZ(0),
 456                                    s5k6aa->pclk_fmax);
 457         if (!ret)
 458                 ret = s5k6aa_write(c, REG_I_INIT_PARAMS_UPDATED, 1);
 459         if (!ret)
 460                 ret = s5k6aa_read(c, REG_I_ERROR_INFO, &status);
 461
 462         return ret ? ret : (status ? -EINVAL : 0);
 463 }
 464
 465 /* Set horizontal and vertical image flipping */
 466 static int s5k6aa_set_mirror(struct s5k6aa *s5k6aa, int horiz_flip)
 467 {
 468         struct i2c_client *client = v4l2_get_subdevdata(&s5k6aa->sd);
 469         int index = s5k6aa->preset->index;
 470
 471         unsigned int vflip = s5k6aa->ctrls.vflip->val ^ s5k6aa->inv_vflip;
 472         unsigned int flip = (horiz_flip ^ s5k6aa->inv_hflip) | (vflip << 1);
 473
 474         return s5k6aa_write(client, REG_P_PREV_MIRROR(index), flip);
 475 }
 476
 477 /* Configure auto/manual white balance and R/G/B gains */
 478 static int s5k6aa_set_awb(struct s5k6aa *s5k6aa, int awb)
 479 {
 480         struct i2c_client *c = v4l2_get_subdevdata(&s5k6aa->sd);
 481         struct s5k6aa_ctrls *ctrls = &s5k6aa->ctrls;
 482         u16 reg;
 483
 484         int ret = s5k6aa_read(c, REG_DBG_AUTOALG_EN, &reg);
 485
 486         if (!ret && !awb) {
 487                 ret = s5k6aa_write(c, REG_SF_RGAIN, ctrls->gain_red->val);
 488                 if (!ret)
 489                         ret = s5k6aa_write(c, REG_SF_RGAIN_CHG, 1);
 490                 if (ret)
 491                         return ret;
 492
 493                 ret = s5k6aa_write(c, REG_SF_GGAIN, ctrls->gain_green->val);
 494                 if (!ret)
 495                         ret = s5k6aa_write(c, REG_SF_GGAIN_CHG, 1);
 496                 if (ret)
 497                         return ret;
 498
 499                 ret = s5k6aa_write(c, REG_SF_BGAIN, ctrls->gain_blue->val);
 500                 if (!ret)
 501                         ret = s5k6aa_write(c, REG_SF_BGAIN_CHG, 1);
 502         }
 503         if (!ret) {
 504                 reg = awb ? reg | AALG_WB_EN_MASK : reg & ~AALG_WB_EN_MASK;
 505                 ret = s5k6aa_write(c, REG_DBG_AUTOALG_EN, reg);
 506         }
 507
 508         return ret;
 509 }
 510
 511 /* Program FW with exposure time, 'exposure' in us units */
 512 static int s5k6aa_set_user_exposure(struct i2c_client *client, int exposure)
 513 {
 514         unsigned int time = exposure / 10;
 515
 516         int ret = s5k6aa_write(client, REG_SF_USR_EXPOSURE_L, time & 0xffff);
 517         if (!ret)
 518                 ret = s5k6aa_write(client, REG_SF_USR_EXPOSURE_H, time >> 16);
 519         if (ret)
 520                 return ret;
 521         return s5k6aa_write(client, REG_SF_USR_EXPOSURE_CHG, 1);
 522 }
 523
 524 static int s5k6aa_set_user_gain(struct i2c_client *client, int gain)
 525 {
 526         int ret = s5k6aa_write(client, REG_SF_USR_TOT_GAIN, gain);
 527         if (ret)
 528                 return ret;
 529         return s5k6aa_write(client, REG_SF_USR_TOT_GAIN_CHG, 1);
 530 }
 531
 532 /* Set auto/manual exposure and total gain */
 533 static int s5k6aa_set_auto_exposure(struct s5k6aa *s5k6aa, int value)
 534 {
 535         struct i2c_client *c = v4l2_get_subdevdata(&s5k6aa->sd);
 536         unsigned int exp_time = s5k6aa->ctrls.exposure->val;
 537         u16 auto_alg;
 538
 539         int ret = s5k6aa_read(c, REG_DBG_AUTOALG_EN, &auto_alg);
 540         if (ret)
 541                 return ret;
 542
 543         v4l2_dbg(1, debug, c, "man_exp: %d, auto_exp: %d, a_alg: 0x%x\n",
 544                  exp_time, value, auto_alg);
 545
 546         if (value == V4L2_EXPOSURE_AUTO) {
 547                 auto_alg |= AALG_AE_EN_MASK | AALG_DIVLEI_EN_MASK;
 548         } else {
 549                 ret = s5k6aa_set_user_exposure(c, exp_time);
 550                 if (ret)
 551                         return ret;
 552                 ret = s5k6aa_set_user_gain(c, s5k6aa->ctrls.gain->val);
 553                 if (ret)
 554                         return ret;
 555                 auto_alg &= ~(AALG_AE_EN_MASK | AALG_DIVLEI_EN_MASK);
 556         }
 557
 558         return s5k6aa_write(c, REG_DBG_AUTOALG_EN, auto_alg);
 559 }
 560
 561 static int s5k6aa_set_anti_flicker(struct s5k6aa *s5k6aa, int value)
 562 {
 563         struct i2c_client *client = v4l2_get_subdevdata(&s5k6aa->sd);
 564         u16 auto_alg;
 565         int ret;
 566
 567         ret = s5k6aa_read(client, REG_DBG_AUTOALG_EN, &auto_alg);
 568         if (ret)
 569                 return ret;
 570
 571         if (value == V4L2_CID_POWER_LINE_FREQUENCY_AUTO) {
 572                 auto_alg |= AALG_FLICKER_EN_MASK;
 573         } else {
 574                 auto_alg &= ~AALG_FLICKER_EN_MASK;
 575                 /* The V4L2_CID_LINE_FREQUENCY control values match
 576                  * the register values */
 577                 ret = s5k6aa_write(client, REG_SF_FLICKER_QUANT, value);
 578                 if (ret)
 579                         return ret;
 580                 ret = s5k6aa_write(client, REG_SF_FLICKER_QUANT_CHG, 1);
 581                 if (ret)
 582                         return ret;
 583         }
 584
 585         return s5k6aa_write(client, REG_DBG_AUTOALG_EN, auto_alg);
 586 }
 587
 588 static int s5k6aa_set_colorfx(struct s5k6aa *s5k6aa, int val)
 589 {
 590         struct i2c_client *client = v4l2_get_subdevdata(&s5k6aa->sd);
 591         static const struct v4l2_control colorfx[] = {
 592                 { V4L2_COLORFX_NONE,     0 },
 593                 { V4L2_COLORFX_BW,       1 },
 594                 { V4L2_COLORFX_NEGATIVE, 2 },
 595                 { V4L2_COLORFX_SEPIA,    3 },
 596                 { V4L2_COLORFX_SKY_BLUE, 4 },
 597                 { V4L2_COLORFX_SKETCH,   5 },
 598         };
 599         int i;
 600
 601         for (i = 0; i < ARRAY_SIZE(colorfx); i++) {
 602                 if (colorfx[i].id == val)
 603                         return s5k6aa_write(client, REG_G_SPEC_EFFECTS,
 604                                             colorfx[i].value);
 605         }
 606         return -EINVAL;
 607 }
 608
 609 static int s5k6aa_preview_config_status(struct i2c_client *client)
 610 {
 611         u16 error = 0;
 612         int ret = s5k6aa_read(client, REG_G_PREV_CFG_ERROR, &error);
 613
 614         v4l2_dbg(1, debug, client, "error: 0x%x (%d)\n", error, ret);
 615         return ret ? ret : (error ? -EINVAL : 0);
 616 }
 617
 618 static int s5k6aa_get_pixfmt_index(struct s5k6aa *s5k6aa,
 619                                    struct v4l2_mbus_framefmt *mf)
 620 {
 621         unsigned int i;
 622
 623         for (i = 0; i < ARRAY_SIZE(s5k6aa_formats); i++)
 624                 if (mf->colorspace == s5k6aa_formats[i].colorspace &&
 625                     mf->code == s5k6aa_formats[i].code)
 626                         return i;
 627         return 0;
 628 }
 629
 630 static int s5k6aa_set_output_framefmt(struct s5k6aa *s5k6aa,
 631                                       struct s5k6aa_preset *preset)
 632 {
 633         struct i2c_client *client = v4l2_get_subdevdata(&s5k6aa->sd);
 634         int fmt_index = s5k6aa_get_pixfmt_index(s5k6aa, &preset->mbus_fmt);
 635         int ret;
 636
 637         ret = s5k6aa_write(client, REG_P_OUT_WIDTH(preset->index),
 638                            preset->mbus_fmt.width);
 639         if (!ret)
 640                 ret = s5k6aa_write(client, REG_P_OUT_HEIGHT(preset->index),
 641                                    preset->mbus_fmt.height);
 642         if (!ret)
 643                 ret = s5k6aa_write(client, REG_P_FMT(preset->index),
 644                                    s5k6aa_formats[fmt_index].reg_p_fmt);
 645         return ret;
 646 }
 647
 648 static int s5k6aa_set_input_params(struct s5k6aa *s5k6aa)
 649 {
 650         struct i2c_client *c = v4l2_get_subdevdata(&s5k6aa->sd);
 651         struct v4l2_rect *r = &s5k6aa->ccd_rect;
 652         int ret;
 653
 654         ret = s5k6aa_write(c, REG_G_PREVZOOM_IN_WIDTH, r->width);
 655         if (!ret)
 656                 ret = s5k6aa_write(c, REG_G_PREVZOOM_IN_HEIGHT, r->height);
 657         if (!ret)
 658                 ret = s5k6aa_write(c, REG_G_PREVZOOM_IN_XOFFS, r->left);
 659         if (!ret)
 660                 ret = s5k6aa_write(c, REG_G_PREVZOOM_IN_YOFFS, r->top);
 661         if (!ret)
 662                 ret = s5k6aa_write(c, REG_G_INPUTS_CHANGE_REQ, 1);
 663         if (!ret)
 664                 s5k6aa->apply_crop = 0;
 665
 666         return ret;
 667 }
 668
 669 /**
 670  * s5k6aa_configure_video_bus - configure the video output interface
 671  * @bus_type: video bus type: parallel or MIPI-CSI
 672  * @nlanes: number of MIPI lanes to be used (MIPI-CSI only)
 673  *
 674  * Note: Only parallel bus operation has been tested.
 675  */
 676 static int s5k6aa_configure_video_bus(struct s5k6aa *s5k6aa,
 677                                       enum v4l2_mbus_type bus_type, int nlanes)
 678 {
 679         struct i2c_client *client = v4l2_get_subdevdata(&s5k6aa->sd);
 680         u16 cfg = 0;
 681         int ret;
 682
 683         /*
 684          * TODO: The sensor is supposed to support BT.601 and BT.656
 685          * but there is nothing indicating how to switch between both
 686          * in the datasheet. For now default BT.601 interface is assumed.
 687          */
 688         if (bus_type == V4L2_MBUS_CSI2)
 689                 cfg = nlanes;
 690         else if (bus_type != V4L2_MBUS_PARALLEL)
 691                 return -EINVAL;
 692
 693         ret = s5k6aa_write(client, REG_OIF_EN_MIPI_LANES, cfg);
 694         if (ret)
 695                 return ret;
 696         return s5k6aa_write(client, REG_OIF_CFG_CHG, 1);
 697 }
 698
 699 /* This function should be called when switching to new user configuration set*/
 700 static int s5k6aa_new_config_sync(struct i2c_client *client, int timeout,
 701                                   int cid)
 702 {
 703         unsigned long end = jiffies + msecs_to_jiffies(timeout);
 704         u16 reg = 1;
 705         int ret;
 706
 707         ret = s5k6aa_write(client, REG_G_ACTIVE_PREV_CFG, cid);
 708         if (!ret)
 709                 ret = s5k6aa_write(client, REG_G_PREV_CFG_CHG, 1);
 710         if (!ret)
 711                 ret = s5k6aa_write(client, REG_G_NEW_CFG_SYNC, 1);
 712         if (timeout == 0)
 713                 return ret;
 714
 715         while (ret >= 0 && time_is_after_jiffies(end)) {
 716                 ret = s5k6aa_read(client, REG_G_NEW_CFG_SYNC, &reg);
 717                 if (!reg)
 718                         return 0;
 719                 usleep_range(1000, 5000);
 720         }
 721         return ret ? ret : -ETIMEDOUT;
 722 }
 723
 724 /**
 725  * s5k6aa_set_prev_config - write user preview register set
 726  *
 727  * Configure output resolution and color fromat, pixel clock
 728  * frequency range, device frame rate type and frame period range.
 729  */
 730 static int s5k6aa_set_prev_config(struct s5k6aa *s5k6aa,
 731                                   struct s5k6aa_preset *preset)
 732 {
 733         struct i2c_client *client = v4l2_get_subdevdata(&s5k6aa->sd);
 734         int idx = preset->index;
 735         u16 frame_rate_q;
 736         int ret;
 737
 738         if (s5k6aa->fiv->reg_fr_time >= S5K6AA_MAX_HIGHRES_FR_TIME)
 739                 frame_rate_q = FR_RATE_Q_BEST_FRRATE;
 740         else
 741                 frame_rate_q = FR_RATE_Q_BEST_QUALITY;
 742
 743         ret = s5k6aa_set_output_framefmt(s5k6aa, preset);
 744         if (!ret)
 745                 ret = s5k6aa_write(client, REG_P_MAX_OUT_RATE(idx),
 746                                    s5k6aa->pclk_fmax);
 747         if (!ret)
 748                 ret = s5k6aa_write(client, REG_P_MIN_OUT_RATE(idx),
 749                                    s5k6aa->pclk_fmin);
 750         if (!ret)
 751                 ret = s5k6aa_write(client, REG_P_CLK_INDEX(idx),
 752                                    preset->clk_id);
 753         if (!ret)
 754                 ret = s5k6aa_write(client, REG_P_FR_RATE_TYPE(idx),
 755                                    FR_RATE_DYNAMIC);
 756         if (!ret)
 757                 ret = s5k6aa_write(client, REG_P_FR_RATE_Q_TYPE(idx),
 758                                    frame_rate_q);
 759         if (!ret)
 760                 ret = s5k6aa_write(client, REG_P_MAX_FR_TIME(idx),
 761                                    s5k6aa->fiv->reg_fr_time + 33);
 762         if (!ret)
 763                 ret = s5k6aa_write(client, REG_P_MIN_FR_TIME(idx),
 764                                    s5k6aa->fiv->reg_fr_time - 33);
 765         if (!ret)
 766                 ret = s5k6aa_new_config_sync(client, 250, idx);
 767         if (!ret)
 768                 ret = s5k6aa_preview_config_status(client);
 769         if (!ret)
 770                 s5k6aa->apply_cfg = 0;
 771
 772         v4l2_dbg(1, debug, client, "Frame interval: %d +/- 3.3ms. (%d)\n",
 773                  s5k6aa->fiv->reg_fr_time, ret);
 774         return ret;
 775 }
 776
 777 /**
 778  * s5k6aa_initialize_isp - basic ISP MCU initialization
 779  *
 780  * Configure AHB addresses for registers read/write; configure PLLs for
 781  * required output pixel clock. The ISP power supply needs to be already
 782  * enabled, with an optional H/W reset.
 783  * Locking: called with s5k6aa.lock mutex held.
 784  */
 785 static int s5k6aa_initialize_isp(struct v4l2_subdev *sd)
 786 {
 787         struct i2c_client *client = v4l2_get_subdevdata(sd);
 788         struct s5k6aa *s5k6aa = to_s5k6aa(sd);
 789         int ret;
 790
 791         s5k6aa->apply_crop = 1;
 792         s5k6aa->apply_cfg = 1;
 793         msleep(100);
 794
 795         ret = s5k6aa_set_ahb_address(client);
 796         if (ret)
 797                 return ret;
 798         ret = s5k6aa_configure_video_bus(s5k6aa, s5k6aa->bus_type,
 799                                          s5k6aa->mipi_lanes);
 800         if (ret)
 801                 return ret;
 802         ret = s5k6aa_write_array(sd, s5k6aa_analog_config);
 803         if (ret)
 804                 return ret;
 805         msleep(20);
 806
 807         return s5k6aa_configure_pixel_clocks(s5k6aa);
 808 }
 809
 810 static int s5k6aa_gpio_set_value(struct s5k6aa *priv, int id, u32 val)
 811 {
 812         if (!gpio_is_valid(priv->gpio[id].gpio))
 813                 return 0;
 814         gpio_set_value(priv->gpio[id].gpio, !!val);
 815         return 1;
 816 }
 817
 818 static int s5k6aa_gpio_assert(struct s5k6aa *priv, int id)
 819 {
 820         return s5k6aa_gpio_set_value(priv, id, priv->gpio[id].level);
 821 }
 822
 823 static int s5k6aa_gpio_deassert(struct s5k6aa *priv, int id)
 824 {
 825         return s5k6aa_gpio_set_value(priv, id, !priv->gpio[id].level);
 826 }
 827
 828 static int __s5k6aa_power_on(struct s5k6aa *s5k6aa)
 829 {
 830         int ret;
 831
 832         ret = regulator_bulk_enable(S5K6AA_NUM_SUPPLIES, s5k6aa->supplies);
 833         if (ret)
 834                 return ret;
 835         if (s5k6aa_gpio_deassert(s5k6aa, STBY))
 836                 usleep_range(150, 200);
 837
 838         if (s5k6aa->s_power)
 839                 ret = s5k6aa->s_power(1);
 840         usleep_range(4000, 4000);
 841
 842         if (s5k6aa_gpio_deassert(s5k6aa, RST))
 843                 msleep(20);
 844
 845         return ret;
 846 }
 847
 848 static int __s5k6aa_power_off(struct s5k6aa *s5k6aa)
 849 {
 850         int ret;
 851
 852         if (s5k6aa_gpio_assert(s5k6aa, RST))
 853                 usleep_range(100, 150);
 854
 855         if (s5k6aa->s_power) {
 856                 ret = s5k6aa->s_power(0);
 857                 if (ret)
 858                         return ret;
 859         }
 860         if (s5k6aa_gpio_assert(s5k6aa, STBY))
 861                 usleep_range(50, 100);
 862         s5k6aa->streaming = 0;
 863
 864         return regulator_bulk_disable(S5K6AA_NUM_SUPPLIES, s5k6aa->supplies);
 865 }
 866
 867 /*
 868  * V4L2 subdev core and video operations
 869  */
 870 static int s5k6aa_set_power(struct v4l2_subdev *sd, int on)
 871 {
 872         struct s5k6aa *s5k6aa = to_s5k6aa(sd);
 873         int ret = 0;
 874
 875         mutex_lock(&s5k6aa->lock);
 876
 877         if (!on == s5k6aa->power) {
 878                 if (on) {
 879                         ret = __s5k6aa_power_on(s5k6aa);
 880                         if (!ret)
 881                                 ret = s5k6aa_initialize_isp(sd);
 882                 } else {
 883                         ret = __s5k6aa_power_off(s5k6aa);
 884                 }
 885
 886                 if (!ret)
 887                         s5k6aa->power += on ? 1 : -1;
 888         }
 889
 890         mutex_unlock(&s5k6aa->lock);
 891
 892         if (!on || ret || s5k6aa->power != 1)
 893                 return ret;
 894
 895         return v4l2_ctrl_handler_setup(sd->ctrl_handler);
 896 }
 897
 898 static int __s5k6aa_stream(struct s5k6aa *s5k6aa, int enable)
 899 {
 900         struct i2c_client *client = v4l2_get_subdevdata(&s5k6aa->sd);
 901         int ret = 0;
 902
 903         ret = s5k6aa_write(client, REG_G_ENABLE_PREV, enable);
 904         if (!ret)
 905                 ret = s5k6aa_write(client, REG_G_ENABLE_PREV_CHG, 1);
 906         if (!ret)
 907                 s5k6aa->streaming = enable;
 908
 909         return ret;
 910 }
 911
 912 static int s5k6aa_s_stream(struct v4l2_subdev *sd, int on)
 913 {
 914         struct s5k6aa *s5k6aa = to_s5k6aa(sd);
 915         int ret = 0;
 916
 917         mutex_lock(&s5k6aa->lock);
 918
 919         if (s5k6aa->streaming == !on) {
 920                 if (!ret && s5k6aa->apply_cfg)
 921                         ret = s5k6aa_set_prev_config(s5k6aa, s5k6aa->preset);
 922                 if (s5k6aa->apply_crop)
 923                         ret = s5k6aa_set_input_params(s5k6aa);
 924                 if (!ret)
 925                         ret = __s5k6aa_stream(s5k6aa, !!on);
 926         }
 927         mutex_unlock(&s5k6aa->lock);
 928
 929         return ret;
 930 }
 931
 932 static int s5k6aa_g_frame_interval(struct v4l2_subdev *sd,
 933                                    struct v4l2_subdev_frame_interval *fi)
 934 {
 935         struct s5k6aa *s5k6aa = to_s5k6aa(sd);
 936
 937         mutex_lock(&s5k6aa->lock);
 938         fi->interval = s5k6aa->fiv->interval;
 939         mutex_unlock(&s5k6aa->lock);
 940
 941         return 0;
 942 }
 943
 944 static int __s5k6aa_set_frame_interval(struct s5k6aa *s5k6aa,
 945                                        struct v4l2_subdev_frame_interval *fi)
 946 {
 947         struct v4l2_mbus_framefmt *mbus_fmt = &s5k6aa->preset->mbus_fmt;
 948         const struct s5k6aa_interval *fiv = &s5k6aa_intervals[0];
 949         unsigned int err, min_err = UINT_MAX;
 950         unsigned int i, fr_time;
 951
 952         if (fi->interval.denominator == 0)
 953                 return -EINVAL;
 954
 955         fr_time = fi->interval.numerator * 10000 / fi->interval.denominator;
 956
 957         for (i = 0; i < ARRAY_SIZE(s5k6aa_intervals); i++) {
 958                 const struct s5k6aa_interval *iv = &s5k6aa_intervals[i];
 959
 960                 if (mbus_fmt->width > iv->size.width ||
 961                     mbus_fmt->height > iv->size.height)
 962                         continue;
 963
 964                 err = abs(iv->reg_fr_time - fr_time);
 965                 if (err < min_err) {
 966                         fiv = iv;
 967                         min_err = err;
 968                 }
 969         }
 970         s5k6aa->fiv = fiv;
 971
 972         v4l2_dbg(1, debug, &s5k6aa->sd, "Changed frame interval to %d us\n",
 973                  fiv->reg_fr_time * 100);
 974         return 0;
 975 }
 976
 977 static int s5k6aa_s_frame_interval(struct v4l2_subdev *sd,
 978                                    struct v4l2_subdev_frame_interval *fi)
 979 {
 980         struct s5k6aa *s5k6aa = to_s5k6aa(sd);
 981         int ret;
 982
 983         v4l2_dbg(1, debug, sd, "Setting %d/%d frame interval\n",
 984                  fi->interval.numerator, fi->interval.denominator);
 985
 986         mutex_lock(&s5k6aa->lock);
 987         ret = __s5k6aa_set_frame_interval(s5k6aa, fi);
 988         s5k6aa->apply_cfg = 1;
 989
 990         mutex_unlock(&s5k6aa->lock);
 991         return ret;
 992 }
 993
 994 /*
 995  * V4L2 subdev pad level and video operations
 996  */
 997 static int s5k6aa_enum_frame_interval(struct v4l2_subdev *sd,
 998                               struct v4l2_subdev_fh *fh,
 999                               struct v4l2_subdev_frame_interval_enum *fie)
1000 {
1001         struct s5k6aa *s5k6aa = to_s5k6aa(sd);
1002         const struct s5k6aa_interval *fi;
1003         int ret = 0;
1004
1005         if (fie->index > ARRAY_SIZE(s5k6aa_intervals))
1006                 return -EINVAL;
1007
1008         v4l_bound_align_image(&fie->width, S5K6AA_WIN_WIDTH_MIN,
1009                               S5K6AA_WIN_WIDTH_MAX, 1,
1010                               &fie->height, S5K6AA_WIN_HEIGHT_MIN,
1011                               S5K6AA_WIN_HEIGHT_MAX, 1, 0);
1012
1013         mutex_lock(&s5k6aa->lock);
1014         fi = &s5k6aa_intervals[fie->index];
1015         if (fie->width > fi->size.width || fie->height > fi->size.height)
1016                 ret = -EINVAL;
1017         else
1018                 fie->interval = fi->interval;
1019         mutex_unlock(&s5k6aa->lock);
1020
1021         return ret;
1022 }
1023
1024 static int s5k6aa_enum_mbus_code(struct v4l2_subdev *sd,
1025                                  struct v4l2_subdev_fh *fh,
1026                                  struct v4l2_subdev_mbus_code_enum *code)
1027 {
1028         if (code->index >= ARRAY_SIZE(s5k6aa_formats))
1029                 return -EINVAL;
1030
1031         code->code = s5k6aa_formats[code->index].code;
1032         return 0;
1033 }
1034
1035 static int s5k6aa_enum_frame_size(struct v4l2_subdev *sd,
1036                                   struct v4l2_subdev_fh *fh,
1037                                   struct v4l2_subdev_frame_size_enum *fse)
1038 {
1039         int i = ARRAY_SIZE(s5k6aa_formats);
1040
1041         if (fse->index > 0)
1042                 return -EINVAL;
1043
1044         while (--i)
1045                 if (fse->code == s5k6aa_formats[i].code)
1046                         break;
1047
1048         fse->code = s5k6aa_formats[i].code;
1049         fse->min_width  = S5K6AA_WIN_WIDTH_MIN;
1050         fse->max_width  = S5K6AA_WIN_WIDTH_MAX;
1051         fse->max_height = S5K6AA_WIN_HEIGHT_MIN;
1052         fse->min_height = S5K6AA_WIN_HEIGHT_MAX;
1053
1054         return 0;
1055 }
1056
1057 static struct v4l2_rect *
1058 __s5k6aa_get_crop_rect(struct s5k6aa *s5k6aa, struct v4l2_subdev_fh *fh,
1059                        enum v4l2_subdev_format_whence which)
1060 {
1061         if (which == V4L2_SUBDEV_FORMAT_ACTIVE)
1062                 return &s5k6aa->ccd_rect;
1063         if (which == V4L2_SUBDEV_FORMAT_TRY)
1064                 return v4l2_subdev_get_try_crop(fh, 0);
1065
1066         return NULL;
1067 }
1068
1069 static void s5k6aa_try_format(struct s5k6aa *s5k6aa,
1070                               struct v4l2_mbus_framefmt *mf)
1071 {
1072         unsigned int index;
1073
1074         v4l_bound_align_image(&mf->width, S5K6AA_WIN_WIDTH_MIN,
1075                               S5K6AA_WIN_WIDTH_MAX, 1,
1076                               &mf->height, S5K6AA_WIN_HEIGHT_MIN,
1077                               S5K6AA_WIN_HEIGHT_MAX, 1, 0);
1078
1079         if (mf->colorspace != V4L2_COLORSPACE_JPEG &&
1080             mf->colorspace != V4L2_COLORSPACE_REC709)
1081                 mf->colorspace = V4L2_COLORSPACE_JPEG;
1082
1083         index = s5k6aa_get_pixfmt_index(s5k6aa, mf);
1084
1085         mf->colorspace  = s5k6aa_formats[index].colorspace;
1086         mf->code        = s5k6aa_formats[index].code;
1087         mf->field       = V4L2_FIELD_NONE;
1088 }
1089
1090 static int s5k6aa_get_fmt(struct v4l2_subdev *sd, struct v4l2_subdev_fh *fh,
1091                           struct v4l2_subdev_format *fmt)
1092 {
1093         struct s5k6aa *s5k6aa = to_s5k6aa(sd);
1094         struct v4l2_mbus_framefmt *mf;
1095
1096         memset(fmt->reserved, 0, sizeof(fmt->reserved));
1097
1098         if (fmt->which == V4L2_SUBDEV_FORMAT_TRY) {
1099                 mf = v4l2_subdev_get_try_format(fh, 0);
1100                 fmt->format = *mf;
1101                 return 0;
1102         }
1103
1104         mutex_lock(&s5k6aa->lock);
1105         fmt->format = s5k6aa->preset->mbus_fmt;
1106         mutex_unlock(&s5k6aa->lock);
1107
1108         return 0;
1109 }
1110
1111 static int s5k6aa_set_fmt(struct v4l2_subdev *sd, struct v4l2_subdev_fh *fh,
1112                           struct v4l2_subdev_format *fmt)
1113 {
1114         struct s5k6aa *s5k6aa = to_s5k6aa(sd);
1115         struct s5k6aa_preset *preset = s5k6aa->preset;
1116         struct v4l2_mbus_framefmt *mf;
1117         struct v4l2_rect *crop;
1118         int ret = 0;
1119
1120         mutex_lock(&s5k6aa->lock);
1121         s5k6aa_try_format(s5k6aa, &fmt->format);
1122
1123         if (fmt->which == V4L2_SUBDEV_FORMAT_TRY) {
1124                 mf = v4l2_subdev_get_try_format(fh, fmt->pad);
1125                 crop = v4l2_subdev_get_try_crop(fh, 0);
1126         } else {
1127                 if (s5k6aa->streaming) {
1128                         ret = -EBUSY;
1129                 } else {
1130                         mf = &preset->mbus_fmt;
1131                         crop = &s5k6aa->ccd_rect;
1132                         s5k6aa->apply_cfg = 1;
1133                 }
1134         }
1135
1136         if (ret == 0) {
1137                 struct v4l2_subdev_frame_interval fiv = {
1138                         .interval = {0, 1}
1139                 };
1140
1141                 *mf = fmt->format;
1142                 /*
1143                  * Make sure the crop window is valid, i.e. its size is
1144                  * greater than the output window, as the ISP supports
1145                  * only down-scaling.
1146                  */
1147                 crop->width = clamp_t(unsigned int, crop->width, mf->width,
1148                                       S5K6AA_WIN_WIDTH_MAX);
1149                 crop->height = clamp_t(unsigned int, crop->height, mf->height,
1150                                        S5K6AA_WIN_HEIGHT_MAX);
1151                 crop->left = clamp_t(unsigned int, crop->left, 0,
1152                                      S5K6AA_WIN_WIDTH_MAX - crop->width);
1153                 crop->top  = clamp_t(unsigned int, crop->top, 0,
1154                                      S5K6AA_WIN_HEIGHT_MAX - crop->height);
1155
1156                 /* Reset to minimum possible frame interval */
1157                 ret = __s5k6aa_set_frame_interval(s5k6aa, &fiv);
1158         }
1159         mutex_unlock(&s5k6aa->lock);
1160
1161         return ret;
1162 }
1163
1164 static int s5k6aa_get_crop(struct v4l2_subdev *sd, struct v4l2_subdev_fh *fh,
1165                            struct v4l2_subdev_crop *crop)
1166 {
1167         struct s5k6aa *s5k6aa = to_s5k6aa(sd);
1168         struct v4l2_rect *rect;
1169
1170         memset(crop->reserved, 0, sizeof(crop->reserved));
1171         mutex_lock(&s5k6aa->lock);
1172
1173         rect = __s5k6aa_get_crop_rect(s5k6aa, fh, crop->which);
1174         if (rect)
1175                 crop->rect = *rect;
1176
1177         mutex_unlock(&s5k6aa->lock);
1178
1179         v4l2_dbg(1, debug, sd, "Current crop rectangle: (%d,%d)/%dx%d\n",
1180                  rect->left, rect->top, rect->width, rect->height);
1181
1182         return 0;
1183 }
1184
1185 static int s5k6aa_set_crop(struct v4l2_subdev *sd, struct v4l2_subdev_fh *fh,
1186                            struct v4l2_subdev_crop *crop)
1187 {
1188         struct s5k6aa *s5k6aa = to_s5k6aa(sd);
1189         struct v4l2_mbus_framefmt *mf;
1190         unsigned int max_x, max_y;
1191         struct v4l2_rect *crop_r;
1192
1193         mutex_lock(&s5k6aa->lock);
1194         crop_r = __s5k6aa_get_crop_rect(s5k6aa, fh, crop->which);
1195
1196         if (crop->which == V4L2_SUBDEV_FORMAT_ACTIVE) {
1197                 mf = &s5k6aa->preset->mbus_fmt;
1198                 s5k6aa->apply_crop = 1;
1199         } else {
1200                 mf = v4l2_subdev_get_try_format(fh, 0);
1201         }
1202         v4l_bound_align_image(&crop->rect.width, mf->width,
1203                               S5K6AA_WIN_WIDTH_MAX, 1,
1204                               &crop->rect.height, mf->height,
1205                               S5K6AA_WIN_HEIGHT_MAX, 1, 0);
1206
1207         max_x = (S5K6AA_WIN_WIDTH_MAX - crop->rect.width) & ~1;
1208         max_y = (S5K6AA_WIN_HEIGHT_MAX - crop->rect.height) & ~1;
1209
1210         crop->rect.left = clamp_t(unsigned int, crop->rect.left, 0, max_x);
1211         crop->rect.top  = clamp_t(unsigned int, crop->rect.top, 0, max_y);
1212
1213         *crop_r = crop->rect;
1214
1215         mutex_unlock(&s5k6aa->lock);
1216
1217         v4l2_dbg(1, debug, sd, "Set crop rectangle: (%d,%d)/%dx%d\n",
1218                  crop_r->left, crop_r->top, crop_r->width, crop_r->height);
1219
1220         return 0;
1221 }
1222
1223 static const struct v4l2_subdev_pad_ops s5k6aa_pad_ops = {
1224         .enum_mbus_code         = s5k6aa_enum_mbus_code,
1225         .enum_frame_size        = s5k6aa_enum_frame_size,
1226         .enum_frame_interval    = s5k6aa_enum_frame_interval,
1227         .get_fmt                = s5k6aa_get_fmt,
1228         .set_fmt                = s5k6aa_set_fmt,
1229         .get_crop               = s5k6aa_get_crop,
1230         .set_crop               = s5k6aa_set_crop,
1231 };
1232
1233 static const struct v4l2_subdev_video_ops s5k6aa_video_ops = {
1234         .g_frame_interval       = s5k6aa_g_frame_interval,
1235         .s_frame_interval       = s5k6aa_s_frame_interval,
1236         .s_stream               = s5k6aa_s_stream,
1237 };
1238
1239 /*
1240  * V4L2 subdev controls
1241  */
1242
1243 static int s5k6aa_s_ctrl(struct v4l2_ctrl *ctrl)
1244 {
1245         struct v4l2_subdev *sd = ctrl_to_sd(ctrl);
1246         struct i2c_client *client = v4l2_get_subdevdata(sd);
1247         struct s5k6aa *s5k6aa = to_s5k6aa(sd);
1248         int idx, err = 0;
1249
1250         v4l2_dbg(1, debug, sd, "ctrl: 0x%x, value: %d\n", ctrl->id, ctrl->val);
1251
1252         mutex_lock(&s5k6aa->lock);
1253         /*
1254          * If the device is not powered up by the host driver do
1255          * not apply any controls to H/W at this time. Instead
1256          * the controls will be restored right after power-up.
1257          */
1258         if (s5k6aa->power == 0)
1259                 goto unlock;
1260         idx = s5k6aa->preset->index;
1261
1262         switch (ctrl->id) {
1263         case V4L2_CID_AUTO_WHITE_BALANCE:
1264                 err = s5k6aa_set_awb(s5k6aa, ctrl->val);
1265                 break;
1266
1267         case V4L2_CID_BRIGHTNESS:
1268                 err = s5k6aa_write(client, REG_USER_BRIGHTNESS, ctrl->val);
1269                 break;
1270
1271         case V4L2_CID_COLORFX:
1272                 err = s5k6aa_set_colorfx(s5k6aa, ctrl->val);
1273                 break;
1274
1275         case V4L2_CID_CONTRAST:
1276                 err = s5k6aa_write(client, REG_USER_CONTRAST, ctrl->val);
1277                 break;
1278
1279         case V4L2_CID_EXPOSURE_AUTO:
1280                 err = s5k6aa_set_auto_exposure(s5k6aa, ctrl->val);
1281                 break;
1282
1283         case V4L2_CID_HFLIP:
1284                 err = s5k6aa_set_mirror(s5k6aa, ctrl->val);
1285                 if (err)
1286                         break;
1287                 err = s5k6aa_write(client, REG_G_PREV_CFG_CHG, 1);
1288                 break;
1289
1290         case V4L2_CID_POWER_LINE_FREQUENCY:
1291                 err = s5k6aa_set_anti_flicker(s5k6aa, ctrl->val);
1292                 break;
1293
1294         case V4L2_CID_SATURATION:
1295                 err = s5k6aa_write(client, REG_USER_SATURATION, ctrl->val);
1296                 break;
1297
1298         case V4L2_CID_SHARPNESS:
1299                 err = s5k6aa_write(client, REG_USER_SHARPBLUR, ctrl->val);
1300                 break;
1301
1302         case V4L2_CID_WHITE_BALANCE_TEMPERATURE:
1303                 err = s5k6aa_write(client, REG_P_COLORTEMP(idx), ctrl->val);
1304                 if (err)
1305                         break;
1306                 err = s5k6aa_write(client, REG_G_PREV_CFG_CHG, 1);
1307                 break;
1308         }
1309 unlock:
1310         mutex_unlock(&s5k6aa->lock);
1311         return err;
1312 }
1313
1314 static const struct v4l2_ctrl_ops s5k6aa_ctrl_ops = {
1315         .s_ctrl = s5k6aa_s_ctrl,
1316 };
1317
1318 static int s5k6aa_log_status(struct v4l2_subdev *sd)
1319 {
1320         v4l2_ctrl_handler_log_status(sd->ctrl_handler, sd->name);
1321         return 0;
1322 }
1323
1324 #define V4L2_CID_RED_GAIN       (V4L2_CTRL_CLASS_CAMERA | 0x1001)
1325 #define V4L2_CID_GREEN_GAIN     (V4L2_CTRL_CLASS_CAMERA | 0x1002)
1326 #define V4L2_CID_BLUE_GAIN      (V4L2_CTRL_CLASS_CAMERA | 0x1003)
1327
1328 static const struct v4l2_ctrl_config s5k6aa_ctrls[] = {
1329         {
1330                 .ops    = &s5k6aa_ctrl_ops,
1331                 .id     = V4L2_CID_RED_GAIN,
1332                 .type   = V4L2_CTRL_TYPE_INTEGER,
1333                 .name   = "Gain, Red",
1334                 .min    = 0,
1335                 .max    = 256,
1336                 .def    = 127,
1337                 .step   = 1,
1338         }, {
1339                 .ops    = &s5k6aa_ctrl_ops,
1340                 .id     = V4L2_CID_GREEN_GAIN,
1341                 .type   = V4L2_CTRL_TYPE_INTEGER,
1342                 .name   = "Gain, Green",
1343                 .min    = 0,
1344                 .max    = 256,
1345                 .def    = 127,
1346                 .step   = 1,
1347         }, {
1348                 .ops    = &s5k6aa_ctrl_ops,
1349                 .id     = V4L2_CID_BLUE_GAIN,
1350                 .type   = V4L2_CTRL_TYPE_INTEGER,
1351                 .name   = "Gain, Blue",
1352                 .min    = 0,
1353                 .max    = 256,
1354                 .def    = 127,
1355                 .step   = 1,
1356         },
1357 };
1358
1359 static int s5k6aa_initialize_ctrls(struct s5k6aa *s5k6aa)
1360 {
1361         const struct v4l2_ctrl_ops *ops = &s5k6aa_ctrl_ops;
1362         struct s5k6aa_ctrls *ctrls = &s5k6aa->ctrls;
1363         struct v4l2_ctrl_handler *hdl = &ctrls->handler;
1364
1365         int ret = v4l2_ctrl_handler_init(hdl, 16);
1366         if (ret)
1367                 return ret;
1368         /* Auto white balance cluster */
1369         ctrls->awb = v4l2_ctrl_new_std(hdl, ops, V4L2_CID_AUTO_WHITE_BALANCE,
1370                                        0, 1, 1, 1);
1371         ctrls->gain_red = v4l2_ctrl_new_custom(hdl, &s5k6aa_ctrls[0], NULL);
1372         ctrls->gain_green = v4l2_ctrl_new_custom(hdl, &s5k6aa_ctrls[1], NULL);
1373         ctrls->gain_blue = v4l2_ctrl_new_custom(hdl, &s5k6aa_ctrls[2], NULL);
1374         v4l2_ctrl_auto_cluster(4, &ctrls->awb, 0, false);
1375
1376         ctrls->hflip = v4l2_ctrl_new_std(hdl, ops, V4L2_CID_HFLIP, 0, 1, 1, 0);
1377         ctrls->vflip = v4l2_ctrl_new_std(hdl, ops, V4L2_CID_VFLIP, 0, 1, 1, 0);
1378         v4l2_ctrl_cluster(2, &ctrls->hflip);
1379
1380         ctrls->auto_exp = v4l2_ctrl_new_std_menu(hdl, ops,
1381                                 V4L2_CID_EXPOSURE_AUTO,
1382                                 V4L2_EXPOSURE_MANUAL, 0, V4L2_EXPOSURE_AUTO);
1383         /* Exposure time: x 1 us */
1384         ctrls->exposure = v4l2_ctrl_new_std(hdl, ops, V4L2_CID_EXPOSURE,
1385                                             0, 6000000U, 1, 100000U);
1386         /* Total gain: 256 <=> 1x */
1387         ctrls->gain = v4l2_ctrl_new_std(hdl, ops, V4L2_CID_GAIN,
1388                                         0, 256, 1, 256);
1389         v4l2_ctrl_auto_cluster(3, &ctrls->auto_exp, 0, false);
1390
1391         v4l2_ctrl_new_std_menu(hdl, ops, V4L2_CID_POWER_LINE_FREQUENCY,
1392                                V4L2_CID_POWER_LINE_FREQUENCY_AUTO, 0,
1393                                V4L2_CID_POWER_LINE_FREQUENCY_AUTO);
1394
1395         v4l2_ctrl_new_std_menu(hdl, ops, V4L2_CID_COLORFX,
1396                                V4L2_COLORFX_SKY_BLUE, ~0x6f, V4L2_COLORFX_NONE);
1397
1398         v4l2_ctrl_new_std(hdl, ops, V4L2_CID_WHITE_BALANCE_TEMPERATURE,
1399                           0, 256, 1, 0);
1400
1401         v4l2_ctrl_new_std(hdl, ops, V4L2_CID_SATURATION, -127, 127, 1, 0);
1402         v4l2_ctrl_new_std(hdl, ops, V4L2_CID_BRIGHTNESS, -127, 127, 1, 0);
1403         v4l2_ctrl_new_std(hdl, ops, V4L2_CID_CONTRAST, -127, 127, 1, 0);
1404         v4l2_ctrl_new_std(hdl, ops, V4L2_CID_SHARPNESS, -127, 127, 1, 0);
1405
1406         if (hdl->error) {
1407                 ret = hdl->error;
1408                 v4l2_ctrl_handler_free(hdl);
1409                 return ret;
1410         }
1411
1412         s5k6aa->sd.ctrl_handler = hdl;
1413         return 0;
1414 }
1415
1416 /*
1417  * V4L2 subdev internal operations
1418  */
1419 static int s5k6aa_open(struct v4l2_subdev *sd, struct v4l2_subdev_fh *fh)
1420 {
1421         struct v4l2_mbus_framefmt *format = v4l2_subdev_get_try_format(fh, 0);
1422         struct v4l2_rect *crop = v4l2_subdev_get_try_crop(fh, 0);
1423
1424         format->colorspace = s5k6aa_formats[0].colorspace;
1425         format->code = s5k6aa_formats[0].code;
1426         format->width = S5K6AA_OUT_WIDTH_DEF;
1427         format->height = S5K6AA_OUT_HEIGHT_DEF;
1428         format->field = V4L2_FIELD_NONE;
1429
1430         crop->width = S5K6AA_WIN_WIDTH_MAX;
1431         crop->height = S5K6AA_WIN_HEIGHT_MAX;
1432         crop->left = 0;
1433         crop->top = 0;
1434
1435         return 0;
1436 }
1437
1438 int s5k6aa_check_fw_revision(struct s5k6aa *s5k6aa)
1439 {
1440         struct i2c_client *client = v4l2_get_subdevdata(&s5k6aa->sd);
1441         u16 api_ver = 0, fw_rev = 0;
1442
1443         int ret = s5k6aa_set_ahb_address(client);
1444
1445         if (!ret)
1446                 ret = s5k6aa_read(client, REG_FW_APIVER, &api_ver);
1447         if (!ret)
1448                 ret = s5k6aa_read(client, REG_FW_REVISION, &fw_rev);
1449         if (ret) {
1450                 v4l2_err(&s5k6aa->sd, "FW revision check failed!\n");
1451                 return ret;
1452         }
1453
1454         v4l2_info(&s5k6aa->sd, "FW API ver.: 0x%X, FW rev.: 0x%X\n",
1455                   api_ver, fw_rev);
1456
1457         return api_ver == S5K6AAFX_FW_APIVER ? 0 : -ENODEV;
1458 }
1459
1460 static int s5k6aa_registered(struct v4l2_subdev *sd)
1461 {
1462         struct s5k6aa *s5k6aa = to_s5k6aa(sd);
1463         int ret;
1464
1465         mutex_lock(&s5k6aa->lock);
1466         ret = __s5k6aa_power_on(s5k6aa);
1467         if (!ret) {
1468                 msleep(100);
1469                 ret = s5k6aa_check_fw_revision(s5k6aa);
1470                 __s5k6aa_power_off(s5k6aa);
1471         }
1472         mutex_unlock(&s5k6aa->lock);
1473
1474         return ret;
1475 }
1476
1477 static const struct v4l2_subdev_internal_ops s5k6aa_subdev_internal_ops = {
1478         .registered = s5k6aa_registered,
1479         .open = s5k6aa_open,
1480 };
1481
1482 static const struct v4l2_subdev_core_ops s5k6aa_core_ops = {
1483         .s_power = s5k6aa_set_power,
1484         .log_status = s5k6aa_log_status,
1485 };
1486
1487 static const struct v4l2_subdev_ops s5k6aa_subdev_ops = {
1488         .core = &s5k6aa_core_ops,
1489         .pad = &s5k6aa_pad_ops,
1490         .video = &s5k6aa_video_ops,
1491 };
1492
1493 /*
1494  * GPIO setup
1495  */
1496 static int s5k6aa_configure_gpio(int nr, int val, const char *name)
1497 {
1498         unsigned long flags = val ? GPIOF_OUT_INIT_HIGH : GPIOF_OUT_INIT_LOW;
1499         int ret;
1500
1501         if (!gpio_is_valid(nr))
1502                 return 0;
1503         ret = gpio_request_one(nr, flags, name);
1504         if (!ret)
1505                 gpio_export(nr, 0);
1506         return ret;
1507 }
1508
1509 static void s5k6aa_free_gpios(struct s5k6aa *s5k6aa)
1510 {
1511         int i;
1512
1513         for (i = 0; i < ARRAY_SIZE(s5k6aa->gpio); i++) {
1514                 if (!gpio_is_valid(s5k6aa->gpio[i].gpio))
1515                         continue;
1516                 gpio_free(s5k6aa->gpio[i].gpio);
1517                 s5k6aa->gpio[i].gpio = -EINVAL;
1518         }
1519 }
1520
1521 static int s5k6aa_configure_gpios(struct s5k6aa *s5k6aa,
1522                                   const struct s5k6aa_platform_data *pdata)
1523 {
1524         const struct s5k6aa_gpio *gpio = &pdata->gpio_stby;
1525         int ret;
1526
1527         s5k6aa->gpio[STBY].gpio = -EINVAL;
1528         s5k6aa->gpio[RST].gpio  = -EINVAL;
1529
1530         ret = s5k6aa_configure_gpio(gpio->gpio, gpio->level, "S5K6AA_STBY");
1531         if (ret) {
1532                 s5k6aa_free_gpios(s5k6aa);
1533                 return ret;
1534         }
1535         s5k6aa->gpio[STBY] = *gpio;
1536         if (gpio_is_valid(gpio->gpio))
1537                 gpio_set_value(gpio->gpio, 0);
1538
1539         gpio = &pdata->gpio_reset;
1540         ret = s5k6aa_configure_gpio(gpio->gpio, gpio->level, "S5K6AA_RST");
1541         if (ret) {
1542                 s5k6aa_free_gpios(s5k6aa);
1543                 return ret;
1544         }
1545         s5k6aa->gpio[RST] = *gpio;
1546         if (gpio_is_valid(gpio->gpio))
1547                 gpio_set_value(gpio->gpio, 0);
1548
1549         return 0;
1550 }
1551
1552 static int s5k6aa_probe(struct i2c_client *client,
1553                         const struct i2c_device_id *id)
1554 {
1555         const struct s5k6aa_platform_data *pdata = client->dev.platform_data;
1556         struct v4l2_subdev *sd;
1557         struct s5k6aa *s5k6aa;
1558         int i, ret;
1559
1560         if (pdata == NULL) {
1561                 dev_err(&client->dev, "Platform data not specified\n");
1562                 return -EINVAL;
1563         }
1564
1565         if (pdata->mclk_frequency == 0) {
1566                 dev_err(&client->dev, "MCLK frequency not specified\n");
1567                 return -EINVAL;
1568         }
1569
1570         s5k6aa = kzalloc(sizeof(*s5k6aa), GFP_KERNEL);
1571         if (!s5k6aa)
1572                 return -ENOMEM;
1573
1574         mutex_init(&s5k6aa->lock);
1575
1576         s5k6aa->mclk_frequency = pdata->mclk_frequency;
1577         s5k6aa->bus_type = pdata->bus_type;
1578         s5k6aa->mipi_lanes = pdata->nlanes;
1579         s5k6aa->s_power = pdata->set_power;
1580         s5k6aa->inv_hflip = pdata->horiz_flip;
1581         s5k6aa->inv_vflip = pdata->vert_flip;
1582
1583         sd = &s5k6aa->sd;
1584         strlcpy(sd->name, DRIVER_NAME, sizeof(sd->name));
1585         v4l2_i2c_subdev_init(sd, client, &s5k6aa_subdev_ops);
1586
1587         sd->internal_ops = &s5k6aa_subdev_internal_ops;
1588         sd->flags |= V4L2_SUBDEV_FL_HAS_DEVNODE;
1589
1590         s5k6aa->pad.flags = MEDIA_PAD_FL_SOURCE;
1591         sd->entity.type = MEDIA_ENT_T_V4L2_SUBDEV_SENSOR;
1592         ret = media_entity_init(&sd->entity, 1, &s5k6aa->pad, 0);
1593         if (ret)
1594                 goto out_err1;
1595
1596         ret = s5k6aa_configure_gpios(s5k6aa, pdata);
1597         if (ret)
1598                 goto out_err2;
1599
1600         for (i = 0; i < S5K6AA_NUM_SUPPLIES; i++)
1601                 s5k6aa->supplies[i].supply = s5k6aa_supply_names[i];
1602
1603         ret = regulator_bulk_get(&client->dev, S5K6AA_NUM_SUPPLIES,
1604                                  s5k6aa->supplies);
1605         if (ret) {
1606                 dev_err(&client->dev, "Failed to get regulators\n");
1607                 goto out_err3;
1608         }
1609
1610         ret = s5k6aa_initialize_ctrls(s5k6aa);
1611         if (ret)
1612                 goto out_err4;
1613
1614         s5k6aa_presets_data_init(s5k6aa);
1615
1616         s5k6aa->ccd_rect.width = S5K6AA_WIN_WIDTH_MAX;
1617         s5k6aa->ccd_rect.height = S5K6AA_WIN_HEIGHT_MAX;
1618         s5k6aa->ccd_rect.left = 0;
1619         s5k6aa->ccd_rect.top = 0;
1620
1621         return 0;
1622
1623 out_err4:
1624         regulator_bulk_free(S5K6AA_NUM_SUPPLIES, s5k6aa->supplies);
1625 out_err3:
1626         s5k6aa_free_gpios(s5k6aa);
1627 out_err2:
1628         media_entity_cleanup(&s5k6aa->sd.entity);
1629 out_err1:
1630         kfree(s5k6aa);
1631         return ret;
1632 }
1633
1634 static int s5k6aa_remove(struct i2c_client *client)
1635 {
1636         struct v4l2_subdev *sd = i2c_get_clientdata(client);
1637         struct s5k6aa *s5k6aa = to_s5k6aa(sd);
1638
1639         v4l2_device_unregister_subdev(sd);
1640         v4l2_ctrl_handler_free(sd->ctrl_handler);
1641         media_entity_cleanup(&sd->entity);
1642         regulator_bulk_free(S5K6AA_NUM_SUPPLIES, s5k6aa->supplies);
1643         s5k6aa_free_gpios(s5k6aa);
1644         kfree(s5k6aa);
1645
1646         return 0;
1647 }
1648
1649 static const struct i2c_device_id s5k6aa_id[] = {
1650         { DRIVER_NAME, 0 },
1651         { },
1652 };
1653 MODULE_DEVICE_TABLE(i2c, s5k6aa_id);
1654
1655
1656 static struct i2c_driver s5k6aa_i2c_driver = {
1657         .driver = {
1658                 .name = DRIVER_NAME
1659         },
1660         .probe          = s5k6aa_probe,
1661         .remove         = s5k6aa_remove,
1662         .id_table       = s5k6aa_id,
1663 };
1664
1665 static int __init s5k6aa_init(void)
1666 {
1667         return i2c_add_driver(&s5k6aa_i2c_driver);
1668 }
1669
1670 static void __exit s5k6aa_exit(void)
1671 {
1672         i2c_del_driver(&s5k6aa_i2c_driver);
1673 }
1674
1675 module_init(s5k6aa_init);
1676 module_exit(s5k6aa_exit);
1677
1678 MODULE_DESCRIPTION("Samsung S5K6AA(FX) SXGA camera driver");
1679 MODULE_AUTHOR("Sylwester Nawrocki <s.nawrocki@samsung.com>");
1680 MODULE_LICENSE("GPL");