drivers/media/i2c/s5k5baf.c

   1 /*
   2  * Driver for Samsung S5K5BAF UXGA 1/5" 2M CMOS Image Sensor
   3  * with embedded SoC ISP.
   4  *
   5  * Copyright (C) 2013, Samsung Electronics Co., Ltd.
   6  * Andrzej Hajda <a.hajda@samsung.com>
   7  *
   8  * Based on S5K6AA driver authored by Sylwester Nawrocki
   9  * Copyright (C) 2013, Samsung Electronics Co., Ltd.
  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 version 2 as
  13  * published by the Free Software Foundation.
  14  */
  15
  16 #include <linux/clk.h>
  17 #include <linux/delay.h>
  18 #include <linux/firmware.h>
  19 #include <linux/gpio.h>
  20 #include <linux/i2c.h>
  21 #include <linux/media.h>
  22 #include <linux/module.h>
  23 #include <linux/of_gpio.h>
  24 #include <linux/regulator/consumer.h>
  25 #include <linux/slab.h>
  26
  27 #include <media/media-entity.h>
  28 #include <media/v4l2-ctrls.h>
  29 #include <media/v4l2-device.h>
  30 #include <media/v4l2-subdev.h>
  31 #include <media/v4l2-mediabus.h>
  32 #include <media/v4l2-of.h>
  33
  34 static int debug;
  35 module_param(debug, int, 0644);
  36
  37 #define S5K5BAF_DRIVER_NAME             "s5k5baf"
  38 #define S5K5BAF_DEFAULT_MCLK_FREQ       24000000U
  39 #define S5K5BAF_CLK_NAME                "mclk"
  40
  41 #define S5K5BAF_FW_FILENAME             "s5k5baf-cfg.bin"
  42 #define S5K5BAF_FW_TAG                  "SF00"
  43 #define S5K5BAG_FW_TAG_LEN              2
  44 #define S5K5BAG_FW_MAX_COUNT            16
  45
  46 #define S5K5BAF_CIS_WIDTH               1600
  47 #define S5K5BAF_CIS_HEIGHT              1200
  48 #define S5K5BAF_WIN_WIDTH_MIN           8
  49 #define S5K5BAF_WIN_HEIGHT_MIN          8
  50 #define S5K5BAF_GAIN_RED_DEF            127
  51 #define S5K5BAF_GAIN_GREEN_DEF          95
  52 #define S5K5BAF_GAIN_BLUE_DEF           180
  53 /* Default number of MIPI CSI-2 data lanes used */
  54 #define S5K5BAF_DEF_NUM_LANES           1
  55
  56 #define AHB_MSB_ADDR_PTR                0xfcfc
  57
  58 /*
  59  * Register interface pages (the most significant word of the address)
  60  */
  61 #define PAGE_IF_HW                      0xd000
  62 #define PAGE_IF_SW                      0x7000
  63
  64 /*
  65  * H/W register Interface (PAGE_IF_HW)
  66  */
  67 #define REG_SW_LOAD_COMPLETE            0x0014
  68 #define REG_CMDWR_PAGE                  0x0028
  69 #define REG_CMDWR_ADDR                  0x002a
  70 #define REG_CMDRD_PAGE                  0x002c
  71 #define REG_CMDRD_ADDR                  0x002e
  72 #define REG_CMD_BUF                     0x0f12
  73 #define REG_SET_HOST_INT                0x1000
  74 #define REG_CLEAR_HOST_INT              0x1030
  75 #define REG_PATTERN_SET                 0x3100
  76 #define REG_PATTERN_WIDTH               0x3118
  77 #define REG_PATTERN_HEIGHT              0x311a
  78 #define REG_PATTERN_PARAM               0x311c
  79
  80 /*
  81  * S/W register interface (PAGE_IF_SW)
  82  */
  83
  84 /* Firmware revision information */
  85 #define REG_FW_APIVER                   0x012e
  86 #define  S5K5BAF_FW_APIVER              0x0001
  87 #define REG_FW_REVISION                 0x0130
  88 #define REG_FW_SENSOR_ID                0x0152
  89
  90 /* Initialization parameters */
  91 /* Master clock frequency in KHz */
  92 #define REG_I_INCLK_FREQ_L              0x01b8
  93 #define REG_I_INCLK_FREQ_H              0x01ba
  94 #define  MIN_MCLK_FREQ_KHZ              6000U
  95 #define  MAX_MCLK_FREQ_KHZ              48000U
  96 #define REG_I_USE_NPVI_CLOCKS           0x01c6
  97 #define  NPVI_CLOCKS                    1
  98 #define REG_I_USE_NMIPI_CLOCKS          0x01c8
  99 #define  NMIPI_CLOCKS                   1
 100 #define REG_I_BLOCK_INTERNAL_PLL_CALC   0x01ca
 101
 102 /* Clock configurations, n = 0..2. REG_I_* frequency unit is 4 kHz. */
 103 #define REG_I_OPCLK_4KHZ(n)             ((n) * 6 + 0x01cc)
 104 #define REG_I_MIN_OUTRATE_4KHZ(n)       ((n) * 6 + 0x01ce)
 105 #define REG_I_MAX_OUTRATE_4KHZ(n)       ((n) * 6 + 0x01d0)
 106 #define  SCLK_PVI_FREQ                  24000
 107 #define  SCLK_MIPI_FREQ                 48000
 108 #define  PCLK_MIN_FREQ                  6000
 109 #define  PCLK_MAX_FREQ                  48000
 110 #define REG_I_USE_REGS_API              0x01de
 111 #define REG_I_INIT_PARAMS_UPDATED       0x01e0
 112 #define REG_I_ERROR_INFO                0x01e2
 113
 114 /* General purpose parameters */
 115 #define REG_USER_BRIGHTNESS             0x01e4
 116 #define REG_USER_CONTRAST               0x01e6
 117 #define REG_USER_SATURATION             0x01e8
 118 #define REG_USER_SHARPBLUR              0x01ea
 119
 120 #define REG_G_SPEC_EFFECTS              0x01ee
 121 #define REG_G_ENABLE_PREV               0x01f0
 122 #define REG_G_ENABLE_PREV_CHG           0x01f2
 123 #define REG_G_NEW_CFG_SYNC              0x01f8
 124 #define REG_G_PREVREQ_IN_WIDTH          0x01fa
 125 #define REG_G_PREVREQ_IN_HEIGHT         0x01fc
 126 #define REG_G_PREVREQ_IN_XOFFS          0x01fe
 127 #define REG_G_PREVREQ_IN_YOFFS          0x0200
 128 #define REG_G_PREVZOOM_IN_WIDTH         0x020a
 129 #define REG_G_PREVZOOM_IN_HEIGHT        0x020c
 130 #define REG_G_PREVZOOM_IN_XOFFS         0x020e
 131 #define REG_G_PREVZOOM_IN_YOFFS         0x0210
 132 #define REG_G_INPUTS_CHANGE_REQ         0x021a
 133 #define REG_G_ACTIVE_PREV_CFG           0x021c
 134 #define REG_G_PREV_CFG_CHG              0x021e
 135 #define REG_G_PREV_OPEN_AFTER_CH        0x0220
 136 #define REG_G_PREV_CFG_ERROR            0x0222
 137 #define  CFG_ERROR_RANGE                0x0b
 138 #define REG_G_PREV_CFG_BYPASS_CHANGED   0x022a
 139 #define REG_G_ACTUAL_P_FR_TIME          0x023a
 140 #define REG_G_ACTUAL_P_OUT_RATE         0x023c
 141 #define REG_G_ACTUAL_C_FR_TIME          0x023e
 142 #define REG_G_ACTUAL_C_OUT_RATE         0x0240
 143
 144 /* Preview control section. n = 0...4. */
 145 #define PREG(n, x)                      ((n) * 0x26 + x)
 146 #define REG_P_OUT_WIDTH(n)              PREG(n, 0x0242)
 147 #define REG_P_OUT_HEIGHT(n)             PREG(n, 0x0244)
 148 #define REG_P_FMT(n)                    PREG(n, 0x0246)
 149 #define REG_P_MAX_OUT_RATE(n)           PREG(n, 0x0248)
 150 #define REG_P_MIN_OUT_RATE(n)           PREG(n, 0x024a)
 151 #define REG_P_PVI_MASK(n)               PREG(n, 0x024c)
 152 #define  PVI_MASK_MIPI                  0x52
 153 #define REG_P_CLK_INDEX(n)              PREG(n, 0x024e)
 154 #define  CLK_PVI_INDEX                  0
 155 #define  CLK_MIPI_INDEX                 NPVI_CLOCKS
 156 #define REG_P_FR_RATE_TYPE(n)           PREG(n, 0x0250)
 157 #define  FR_RATE_DYNAMIC                0
 158 #define  FR_RATE_FIXED                  1
 159 #define  FR_RATE_FIXED_ACCURATE         2
 160 #define REG_P_FR_RATE_Q_TYPE(n)         PREG(n, 0x0252)
 161 #define  FR_RATE_Q_DYNAMIC              0
 162 #define  FR_RATE_Q_BEST_FRRATE          1 /* Binning enabled */
 163 #define  FR_RATE_Q_BEST_QUALITY         2 /* Binning disabled */
 164 /* Frame period in 0.1 ms units */
 165 #define REG_P_MAX_FR_TIME(n)            PREG(n, 0x0254)
 166 #define REG_P_MIN_FR_TIME(n)            PREG(n, 0x0256)
 167 #define  S5K5BAF_MIN_FR_TIME            333  /* x100 us */
 168 #define  S5K5BAF_MAX_FR_TIME            6500 /* x100 us */
 169 /* The below 5 registers are for "device correction" values */
 170 #define REG_P_SATURATION(n)             PREG(n, 0x0258)
 171 #define REG_P_SHARP_BLUR(n)             PREG(n, 0x025a)
 172 #define REG_P_GLAMOUR(n)                PREG(n, 0x025c)
 173 #define REG_P_COLORTEMP(n)              PREG(n, 0x025e)
 174 #define REG_P_GAMMA_INDEX(n)            PREG(n, 0x0260)
 175 #define REG_P_PREV_MIRROR(n)            PREG(n, 0x0262)
 176 #define REG_P_CAP_MIRROR(n)             PREG(n, 0x0264)
 177 #define REG_P_CAP_ROTATION(n)           PREG(n, 0x0266)
 178
 179 /* Extended image property controls */
 180 /* Exposure time in 10 us units */
 181 #define REG_SF_USR_EXPOSURE_L           0x03bc
 182 #define REG_SF_USR_EXPOSURE_H           0x03be
 183 #define REG_SF_USR_EXPOSURE_CHG         0x03c0
 184 #define REG_SF_USR_TOT_GAIN             0x03c2
 185 #define REG_SF_USR_TOT_GAIN_CHG         0x03c4
 186 #define REG_SF_RGAIN                    0x03c6
 187 #define REG_SF_RGAIN_CHG                0x03c8
 188 #define REG_SF_GGAIN                    0x03ca
 189 #define REG_SF_GGAIN_CHG                0x03cc
 190 #define REG_SF_BGAIN                    0x03ce
 191 #define REG_SF_BGAIN_CHG                0x03d0
 192 #define REG_SF_WBGAIN_CHG               0x03d2
 193 #define REG_SF_FLICKER_QUANT            0x03d4
 194 #define REG_SF_FLICKER_QUANT_CHG        0x03d6
 195
 196 /* Output interface (parallel/MIPI) setup */
 197 #define REG_OIF_EN_MIPI_LANES           0x03f2
 198 #define REG_OIF_EN_PACKETS              0x03f4
 199 #define  EN_PACKETS_CSI2                0xc3
 200 #define REG_OIF_CFG_CHG                 0x03f6
 201
 202 /* Auto-algorithms enable mask */
 203 #define REG_DBG_AUTOALG_EN              0x03f8
 204 #define  AALG_ALL_EN                    BIT(0)
 205 #define  AALG_AE_EN                     BIT(1)
 206 #define  AALG_DIVLEI_EN                 BIT(2)
 207 #define  AALG_WB_EN                     BIT(3)
 208 #define  AALG_USE_WB_FOR_ISP            BIT(4)
 209 #define  AALG_FLICKER_EN                BIT(5)
 210 #define  AALG_FIT_EN                    BIT(6)
 211 #define  AALG_WRHW_EN                   BIT(7)
 212
 213 /* Pointers to color correction matrices */
 214 #define REG_PTR_CCM_HORIZON             0x06d0
 215 #define REG_PTR_CCM_INCANDESCENT        0x06d4
 216 #define REG_PTR_CCM_WARM_WHITE          0x06d8
 217 #define REG_PTR_CCM_COOL_WHITE          0x06dc
 218 #define REG_PTR_CCM_DL50                0x06e0
 219 #define REG_PTR_CCM_DL65                0x06e4
 220 #define REG_PTR_CCM_OUTDOOR             0x06ec
 221
 222 #define REG_ARR_CCM(n)                  (0x2800 + 36 * (n))
 223
 224 static const char * const s5k5baf_supply_names[] = {
 225         "vdda",         /* Analog power supply 2.8V (2.6V to 3.0V) */
 226         "vddreg",       /* Regulator input power supply 1.8V (1.7V to 1.9V)
 227                            or 2.8V (2.6V to 3.0) */
 228         "vddio",        /* I/O power supply 1.8V (1.65V to 1.95V)
 229                            or 2.8V (2.5V to 3.1V) */
 230 };
 231 #define S5K5BAF_NUM_SUPPLIES ARRAY_SIZE(s5k5baf_supply_names)
 232
 233 struct s5k5baf_gpio {
 234         int gpio;
 235         int level;
 236 };
 237
 238 enum s5k5baf_gpio_id {
 239         STBY,
 240         RST,
 241         NUM_GPIOS,
 242 };
 243
 244 #define PAD_CIS 0
 245 #define PAD_OUT 1
 246 #define NUM_CIS_PADS 1
 247 #define NUM_ISP_PADS 2
 248
 249 struct s5k5baf_pixfmt {
 250         enum v4l2_mbus_pixelcode code;
 251         u32 colorspace;
 252         /* REG_P_FMT(x) register value */
 253         u16 reg_p_fmt;
 254 };
 255
 256 struct s5k5baf_ctrls {
 257         struct v4l2_ctrl_handler handler;
 258         struct { /* Auto / manual white balance cluster */
 259                 struct v4l2_ctrl *awb;
 260                 struct v4l2_ctrl *gain_red;
 261                 struct v4l2_ctrl *gain_blue;
 262         };
 263         struct { /* Mirror cluster */
 264                 struct v4l2_ctrl *hflip;
 265                 struct v4l2_ctrl *vflip;
 266         };
 267         struct { /* Auto exposure / manual exposure and gain cluster */
 268                 struct v4l2_ctrl *auto_exp;
 269                 struct v4l2_ctrl *exposure;
 270                 struct v4l2_ctrl *gain;
 271         };
 272 };
 273
 274 enum {
 275         S5K5BAF_FW_ID_PATCH,
 276         S5K5BAF_FW_ID_CCM,
 277         S5K5BAF_FW_ID_CIS,
 278 };
 279
 280 struct s5k5baf_fw {
 281         u16 count;
 282         struct {
 283                 u16 id;
 284                 u16 offset;
 285         } seq[0];
 286         u16 data[0];
 287 };
 288
 289 struct s5k5baf {
 290         struct s5k5baf_gpio gpios[NUM_GPIOS];
 291         enum v4l2_mbus_type bus_type;
 292         u8 nlanes;
 293         struct regulator_bulk_data supplies[S5K5BAF_NUM_SUPPLIES];
 294
 295         struct clk *clock;
 296         u32 mclk_frequency;
 297
 298         struct s5k5baf_fw *fw;
 299
 300         struct v4l2_subdev cis_sd;
 301         struct media_pad cis_pad;
 302
 303         struct v4l2_subdev sd;
 304         struct media_pad pads[NUM_ISP_PADS];
 305
 306         /* protects the struct members below */
 307         struct mutex lock;
 308
 309         int error;
 310
 311         struct v4l2_rect crop_sink;
 312         struct v4l2_rect compose;
 313         struct v4l2_rect crop_source;
 314         /* index to s5k5baf_formats array */
 315         int pixfmt;
 316         /* actual frame interval in 100us */
 317         u16 fiv;
 318         /* requested frame interval in 100us */
 319         u16 req_fiv;
 320         /* cache for REG_DBG_AUTOALG_EN register */
 321         u16 auto_alg;
 322
 323         struct s5k5baf_ctrls ctrls;
 324
 325         unsigned int streaming:1;
 326         unsigned int apply_cfg:1;
 327         unsigned int apply_crop:1;
 328         unsigned int valid_auto_alg:1;
 329         unsigned int power;
 330 };
 331
 332 static const struct s5k5baf_pixfmt s5k5baf_formats[] = {
 333         { V4L2_MBUS_FMT_VYUY8_2X8,      V4L2_COLORSPACE_JPEG,   5 },
 334         /* range 16-240 */
 335         { V4L2_MBUS_FMT_VYUY8_2X8,      V4L2_COLORSPACE_REC709, 6 },
 336         { V4L2_MBUS_FMT_RGB565_2X8_BE,  V4L2_COLORSPACE_JPEG,   0 },
 337 };
 338
 339 static struct v4l2_rect s5k5baf_cis_rect = {
 340         0, 0, S5K5BAF_CIS_WIDTH, S5K5BAF_CIS_HEIGHT
 341 };
 342
 343 /* Setfile contains set of I2C command sequences. Each sequence has its ID.
 344  * setfile format:
 345  *      u8 magic[4];
 346  *      u16 count;              number of sequences
 347  *      struct {
 348  *              u16 id;         sequence id
 349  *              u16 offset;     sequence offset in data array
 350  *      } seq[count];
 351  *      u16 data[*];            array containing sequences
 352  *
 353  */
 354 static int s5k5baf_fw_parse(struct device *dev, struct s5k5baf_fw **fw,
 355                             size_t count, const u16 *data)
 356 {
 357         struct s5k5baf_fw *f;
 358         u16 *d, i, *end;
 359         int ret;
 360
 361         if (count < S5K5BAG_FW_TAG_LEN + 1) {
 362                 dev_err(dev, "firmware file too short (%zu)\n", count);
 363                 return -EINVAL;
 364         }
 365
 366         ret = memcmp(data, S5K5BAF_FW_TAG, S5K5BAG_FW_TAG_LEN * sizeof(u16));
 367         if (ret != 0) {
 368                 dev_err(dev, "invalid firmware magic number\n");
 369                 return -EINVAL;
 370         }
 371
 372         data += S5K5BAG_FW_TAG_LEN;
 373         count -= S5K5BAG_FW_TAG_LEN;
 374
 375         d = devm_kzalloc(dev, count * sizeof(u16), GFP_KERNEL);
 376
 377         for (i = 0; i < count; ++i)
 378                 d[i] = le16_to_cpu(data[i]);
 379
 380         f = (struct s5k5baf_fw *)d;
 381         if (count < 1 + 2 * f->count) {
 382                 dev_err(dev, "invalid firmware header (count=%d size=%zu)\n",
 383                         f->count, 2 * (count + S5K5BAG_FW_TAG_LEN));
 384                 return -EINVAL;
 385         }
 386         end = d + count;
 387         d += 1 + 2 * f->count;
 388
 389         for (i = 0; i < f->count; ++i) {
 390                 if (f->seq[i].offset + d <= end)
 391                         continue;
 392                 dev_err(dev, "invalid firmware header (seq=%d)\n", i);
 393                 return -EINVAL;
 394         }
 395
 396         *fw = f;
 397
 398         return 0;
 399 }
 400
 401 static inline struct v4l2_subdev *ctrl_to_sd(struct v4l2_ctrl *ctrl)
 402 {
 403         return &container_of(ctrl->handler, struct s5k5baf, ctrls.handler)->sd;
 404 }
 405
 406 static inline bool s5k5baf_is_cis_subdev(struct v4l2_subdev *sd)
 407 {
 408         return sd->entity.type == MEDIA_ENT_T_V4L2_SUBDEV_SENSOR;
 409 }
 410
 411 static inline struct s5k5baf *to_s5k5baf(struct v4l2_subdev *sd)
 412 {
 413         if (s5k5baf_is_cis_subdev(sd))
 414                 return container_of(sd, struct s5k5baf, cis_sd);
 415         else
 416                 return container_of(sd, struct s5k5baf, sd);
 417 }
 418
 419 static u16 s5k5baf_i2c_read(struct s5k5baf *state, u16 addr)
 420 {
 421         struct i2c_client *c = v4l2_get_subdevdata(&state->sd);
 422         __be16 w, r;
 423         struct i2c_msg msg[] = {
 424                 { .addr = c->addr, .flags = 0,
 425                   .len = 2, .buf = (u8 *)&w },
 426                 { .addr = c->addr, .flags = I2C_M_RD,
 427                   .len = 2, .buf = (u8 *)&r },
 428         };
 429         int ret;
 430
 431         if (state->error)
 432                 return 0;
 433
 434         w = cpu_to_be16(addr);
 435         ret = i2c_transfer(c->adapter, msg, 2);
 436         r = be16_to_cpu(r);
 437
 438         v4l2_dbg(3, debug, c, "i2c_read: 0x%04x : 0x%04x\n", addr, r);
 439
 440         if (ret != 2) {
 441                 v4l2_err(c, "i2c_read: error during transfer (%d)\n", ret);
 442                 state->error = ret;
 443         }
 444         return r;
 445 }
 446
 447 static void s5k5baf_i2c_write(struct s5k5baf *state, u16 addr, u16 val)
 448 {
 449         u8 buf[4] = { addr >> 8, addr & 0xFF, val >> 8, val & 0xFF };
 450         struct i2c_client *c = v4l2_get_subdevdata(&state->sd);
 451         int ret;
 452
 453         if (state->error)
 454                 return;
 455
 456         ret = i2c_master_send(c, buf, 4);
 457         v4l2_dbg(3, debug, c, "i2c_write: 0x%04x : 0x%04x\n", addr, val);
 458
 459         if (ret != 4) {
 460                 v4l2_err(c, "i2c_write: error during transfer (%d)\n", ret);
 461                 state->error = ret;
 462         }
 463 }
 464
 465 static u16 s5k5baf_read(struct s5k5baf *state, u16 addr)
 466 {
 467         s5k5baf_i2c_write(state, REG_CMDRD_ADDR, addr);
 468         return s5k5baf_i2c_read(state, REG_CMD_BUF);
 469 }
 470
 471 static void s5k5baf_write(struct s5k5baf *state, u16 addr, u16 val)
 472 {
 473         s5k5baf_i2c_write(state, REG_CMDWR_ADDR, addr);
 474         s5k5baf_i2c_write(state, REG_CMD_BUF, val);
 475 }
 476
 477 static void s5k5baf_write_arr_seq(struct s5k5baf *state, u16 addr,
 478                                   u16 count, const u16 *seq)
 479 {
 480         struct i2c_client *c = v4l2_get_subdevdata(&state->sd);
 481         __be16 buf[65];
 482
 483         s5k5baf_i2c_write(state, REG_CMDWR_ADDR, addr);
 484         if (state->error)
 485                 return;
 486
 487         v4l2_dbg(3, debug, c, "i2c_write_seq(count=%d): %*ph\n", count,
 488                  min(2 * count, 64), seq);
 489
 490         buf[0] = __constant_cpu_to_be16(REG_CMD_BUF);
 491
 492         while (count > 0) {
 493                 int n = min_t(int, count, ARRAY_SIZE(buf) - 1);
 494                 int ret, i;
 495
 496                 for (i = 1; i <= n; ++i)
 497                         buf[i] = cpu_to_be16(*seq++);
 498
 499                 i *= 2;
 500                 ret = i2c_master_send(c, (char *)buf, i);
 501                 if (ret != i) {
 502                         v4l2_err(c, "i2c_write_seq: error during transfer (%d)\n", ret);
 503                         state->error = ret;
 504                         break;
 505                 }
 506
 507                 count -= n;
 508         }
 509 }
 510
 511 #define s5k5baf_write_seq(state, addr, seq...) \
 512         s5k5baf_write_arr_seq(state, addr, sizeof((char[]){ seq }), \
 513                               (const u16 []){ seq });
 514
 515 /* add items count at the beginning of the list */
 516 #define NSEQ(seq...) sizeof((char[]){ seq }), seq
 517
 518 /*
 519  * s5k5baf_write_nseq() - Writes sequences of values to sensor memory via i2c
 520  * @nseq: sequence of u16 words in format:
 521  *      (N, address, value[1]...value[N-1])*,0
 522  * Ex.:
 523  *      u16 seq[] = { NSEQ(0x4000, 1, 1), NSEQ(0x4010, 640, 480), 0 };
 524  *      ret = s5k5baf_write_nseq(c, seq);
 525  */
 526 static void s5k5baf_write_nseq(struct s5k5baf *state, const u16 *nseq)
 527 {
 528         int count;
 529
 530         while ((count = *nseq++)) {
 531                 u16 addr = *nseq++;
 532                 --count;
 533
 534                 s5k5baf_write_arr_seq(state, addr, count, nseq);
 535                 nseq += count;
 536         }
 537 }
 538
 539 static void s5k5baf_synchronize(struct s5k5baf *state, int timeout, u16 addr)
 540 {
 541         unsigned long end = jiffies + msecs_to_jiffies(timeout);
 542         u16 reg;
 543
 544         s5k5baf_write(state, addr, 1);
 545         do {
 546                 reg = s5k5baf_read(state, addr);
 547                 if (state->error || !reg)
 548                         return;
 549                 usleep_range(5000, 10000);
 550         } while (time_is_after_jiffies(end));
 551
 552         v4l2_err(&state->sd, "timeout on register synchronize (%#x)\n", addr);
 553         state->error = -ETIMEDOUT;
 554 }
 555
 556 static u16 *s5k5baf_fw_get_seq(struct s5k5baf *state, u16 seq_id)
 557 {
 558         struct s5k5baf_fw *fw = state->fw;
 559         u16 *data;
 560         int i;
 561
 562         if (fw == NULL)
 563                 return NULL;
 564
 565         data = fw->data + 2 * fw->count;
 566
 567         for (i = 0; i < fw->count; ++i) {
 568                 if (fw->seq[i].id == seq_id)
 569                         return data + fw->seq[i].offset;
 570         }
 571
 572         return NULL;
 573 }
 574
 575 static void s5k5baf_hw_patch(struct s5k5baf *state)
 576 {
 577         u16 *seq = s5k5baf_fw_get_seq(state, S5K5BAF_FW_ID_PATCH);
 578
 579         if (seq)
 580                 s5k5baf_write_nseq(state, seq);
 581 }
 582
 583 static void s5k5baf_hw_set_clocks(struct s5k5baf *state)
 584 {
 585         unsigned long mclk = state->mclk_frequency / 1000;
 586         u16 status;
 587         static const u16 nseq_clk_cfg[] = {
 588                 NSEQ(REG_I_USE_NPVI_CLOCKS,
 589                   NPVI_CLOCKS, NMIPI_CLOCKS, 0,
 590                   SCLK_PVI_FREQ / 4, PCLK_MIN_FREQ / 4, PCLK_MAX_FREQ / 4,
 591                   SCLK_MIPI_FREQ / 4, PCLK_MIN_FREQ / 4, PCLK_MAX_FREQ / 4),
 592                 NSEQ(REG_I_USE_REGS_API, 1),
 593                 0
 594         };
 595
 596         s5k5baf_write_seq(state, REG_I_INCLK_FREQ_L, mclk & 0xffff, mclk >> 16);
 597         s5k5baf_write_nseq(state, nseq_clk_cfg);
 598
 599         s5k5baf_synchronize(state, 250, REG_I_INIT_PARAMS_UPDATED);
 600         status = s5k5baf_read(state, REG_I_ERROR_INFO);
 601         if (!state->error && status) {
 602                 v4l2_err(&state->sd, "error configuring PLL (%d)\n", status);
 603                 state->error = -EINVAL;
 604         }
 605 }
 606
 607 /* set custom color correction matrices for various illuminations */
 608 static void s5k5baf_hw_set_ccm(struct s5k5baf *state)
 609 {
 610         u16 *seq = s5k5baf_fw_get_seq(state, S5K5BAF_FW_ID_CCM);
 611
 612         if (seq)
 613                 s5k5baf_write_nseq(state, seq);
 614 }
 615
 616 /* CIS sensor tuning, based on undocumented android driver code */
 617 static void s5k5baf_hw_set_cis(struct s5k5baf *state)
 618 {
 619         u16 *seq = s5k5baf_fw_get_seq(state, S5K5BAF_FW_ID_CIS);
 620
 621         if (!seq)
 622                 return;
 623
 624         s5k5baf_i2c_write(state, REG_CMDWR_PAGE, PAGE_IF_HW);
 625         s5k5baf_write_nseq(state, seq);
 626         s5k5baf_i2c_write(state, REG_CMDWR_PAGE, PAGE_IF_SW);
 627 }
 628
 629 static void s5k5baf_hw_sync_cfg(struct s5k5baf *state)
 630 {
 631         s5k5baf_write(state, REG_G_PREV_CFG_CHG, 1);
 632         if (state->apply_crop) {
 633                 s5k5baf_write(state, REG_G_INPUTS_CHANGE_REQ, 1);
 634                 s5k5baf_write(state, REG_G_PREV_CFG_BYPASS_CHANGED, 1);
 635         }
 636         s5k5baf_synchronize(state, 500, REG_G_NEW_CFG_SYNC);
 637 }
 638 /* Set horizontal and vertical image flipping */
 639 static void s5k5baf_hw_set_mirror(struct s5k5baf *state)
 640 {
 641         u16 flip = state->ctrls.vflip->val | (state->ctrls.vflip->val << 1);
 642
 643         s5k5baf_write(state, REG_P_PREV_MIRROR(0), flip);
 644         if (state->streaming)
 645                 s5k5baf_hw_sync_cfg(state);
 646 }
 647
 648 static void s5k5baf_hw_set_alg(struct s5k5baf *state, u16 alg, bool enable)
 649 {
 650         u16 cur_alg, new_alg;
 651
 652         if (!state->valid_auto_alg)
 653                 cur_alg = s5k5baf_read(state, REG_DBG_AUTOALG_EN);
 654         else
 655                 cur_alg = state->auto_alg;
 656
 657         new_alg = enable ? (cur_alg | alg) : (cur_alg & ~alg);
 658
 659         if (new_alg != cur_alg)
 660                 s5k5baf_write(state, REG_DBG_AUTOALG_EN, new_alg);
 661
 662         if (state->error)
 663                 return;
 664
 665         state->valid_auto_alg = 1;
 666         state->auto_alg = new_alg;
 667 }
 668
 669 /* Configure auto/manual white balance and R/G/B gains */
 670 static void s5k5baf_hw_set_awb(struct s5k5baf *state, int awb)
 671 {
 672         struct s5k5baf_ctrls *ctrls = &state->ctrls;
 673
 674         if (!awb)
 675                 s5k5baf_write_seq(state, REG_SF_RGAIN,
 676                                   ctrls->gain_red->val, 1,
 677                                   S5K5BAF_GAIN_GREEN_DEF, 1,
 678                                   ctrls->gain_blue->val, 1,
 679                                   1);
 680
 681         s5k5baf_hw_set_alg(state, AALG_WB_EN, awb);
 682 }
 683
 684 /* Program FW with exposure time, 'exposure' in us units */
 685 static void s5k5baf_hw_set_user_exposure(struct s5k5baf *state, int exposure)
 686 {
 687         unsigned int time = exposure / 10;
 688
 689         s5k5baf_write_seq(state, REG_SF_USR_EXPOSURE_L,
 690                           time & 0xffff, time >> 16, 1);
 691 }
 692
 693 static void s5k5baf_hw_set_user_gain(struct s5k5baf *state, int gain)
 694 {
 695         s5k5baf_write_seq(state, REG_SF_USR_TOT_GAIN, gain, 1);
 696 }
 697
 698 /* Set auto/manual exposure and total gain */
 699 static void s5k5baf_hw_set_auto_exposure(struct s5k5baf *state, int value)
 700 {
 701         if (value == V4L2_EXPOSURE_AUTO) {
 702                 s5k5baf_hw_set_alg(state, AALG_AE_EN | AALG_DIVLEI_EN, true);
 703         } else {
 704                 unsigned int exp_time = state->ctrls.exposure->val;
 705
 706                 s5k5baf_hw_set_user_exposure(state, exp_time);
 707                 s5k5baf_hw_set_user_gain(state, state->ctrls.gain->val);
 708                 s5k5baf_hw_set_alg(state, AALG_AE_EN | AALG_DIVLEI_EN, false);
 709         }
 710 }
 711
 712 static void s5k5baf_hw_set_anti_flicker(struct s5k5baf *state, int v)
 713 {
 714         if (v == V4L2_CID_POWER_LINE_FREQUENCY_AUTO) {
 715                 s5k5baf_hw_set_alg(state, AALG_FLICKER_EN, true);
 716         } else {
 717                 /* The V4L2_CID_LINE_FREQUENCY control values match
 718                  * the register values */
 719                 s5k5baf_write_seq(state, REG_SF_FLICKER_QUANT, v, 1);
 720                 s5k5baf_hw_set_alg(state, AALG_FLICKER_EN, false);
 721         }
 722 }
 723
 724 static void s5k5baf_hw_set_colorfx(struct s5k5baf *state, int val)
 725 {
 726         static const u16 colorfx[] = {
 727                 [V4L2_COLORFX_NONE] = 0,
 728                 [V4L2_COLORFX_BW] = 1,
 729                 [V4L2_COLORFX_NEGATIVE] = 2,
 730                 [V4L2_COLORFX_SEPIA] = 3,
 731                 [V4L2_COLORFX_SKY_BLUE] = 4,
 732                 [V4L2_COLORFX_SKETCH] = 5,
 733         };
 734
 735         s5k5baf_write(state, REG_G_SPEC_EFFECTS, colorfx[val]);
 736 }
 737
 738 static int s5k5baf_find_pixfmt(struct v4l2_mbus_framefmt *mf)
 739 {
 740         int i, c = -1;
 741
 742         for (i = 0; i < ARRAY_SIZE(s5k5baf_formats); i++) {
 743                 if (mf->colorspace != s5k5baf_formats[i].colorspace)
 744                         continue;
 745                 if (mf->code == s5k5baf_formats[i].code)
 746                         return i;
 747                 if (c < 0)
 748                         c = i;
 749         }
 750         return (c < 0) ? 0 : c;
 751 }
 752
 753 static int s5k5baf_clear_error(struct s5k5baf *state)
 754 {
 755         int ret = state->error;
 756
 757         state->error = 0;
 758         return ret;
 759 }
 760
 761 static int s5k5baf_hw_set_video_bus(struct s5k5baf *state)
 762 {
 763         u16 en_pkts;
 764
 765         if (state->bus_type == V4L2_MBUS_CSI2)
 766                 en_pkts = EN_PACKETS_CSI2;
 767         else
 768                 en_pkts = 0;
 769
 770         s5k5baf_write_seq(state, REG_OIF_EN_MIPI_LANES,
 771                           state->nlanes, en_pkts, 1);
 772
 773         return s5k5baf_clear_error(state);
 774 }
 775
 776 static u16 s5k5baf_get_cfg_error(struct s5k5baf *state)
 777 {
 778         u16 err = s5k5baf_read(state, REG_G_PREV_CFG_ERROR);
 779         if (err)
 780                 s5k5baf_write(state, REG_G_PREV_CFG_ERROR, 0);
 781         return err;
 782 }
 783
 784 static void s5k5baf_hw_set_fiv(struct s5k5baf *state, u16 fiv)
 785 {
 786         s5k5baf_write(state, REG_P_MAX_FR_TIME(0), fiv);
 787         s5k5baf_hw_sync_cfg(state);
 788 }
 789
 790 static void s5k5baf_hw_find_min_fiv(struct s5k5baf *state)
 791 {
 792         u16 err, fiv;
 793         int n;
 794
 795         fiv = s5k5baf_read(state,  REG_G_ACTUAL_P_FR_TIME);
 796         if (state->error)
 797                 return;
 798
 799         for (n = 5; n > 0; --n) {
 800                 s5k5baf_hw_set_fiv(state, fiv);
 801                 err = s5k5baf_get_cfg_error(state);
 802                 if (state->error)
 803                         return;
 804                 switch (err) {
 805                 case CFG_ERROR_RANGE:
 806                         ++fiv;
 807                         break;
 808                 case 0:
 809                         state->fiv = fiv;
 810                         v4l2_info(&state->sd,
 811                                   "found valid frame interval: %d00us\n", fiv);
 812                         return;
 813                 default:
 814                         v4l2_err(&state->sd,
 815                                  "error setting frame interval: %d\n", err);
 816                         state->error = -EINVAL;
 817                 }
 818         };
 819         v4l2_err(&state->sd, "cannot find correct frame interval\n");
 820         state->error = -ERANGE;
 821 }
 822
 823 static void s5k5baf_hw_validate_cfg(struct s5k5baf *state)
 824 {
 825         u16 err;
 826
 827         err = s5k5baf_get_cfg_error(state);
 828         if (state->error)
 829                 return;
 830
 831         switch (err) {
 832         case 0:
 833                 state->apply_cfg = 1;
 834                 return;
 835         case CFG_ERROR_RANGE:
 836                 s5k5baf_hw_find_min_fiv(state);
 837                 if (!state->error)
 838                         state->apply_cfg = 1;
 839                 return;
 840         default:
 841                 v4l2_err(&state->sd,
 842                          "error setting format: %d\n", err);
 843                 state->error = -EINVAL;
 844         }
 845 }
 846
 847 static void s5k5baf_rescale(struct v4l2_rect *r, const struct v4l2_rect *v,
 848                             const struct v4l2_rect *n,
 849                             const struct v4l2_rect *d)
 850 {
 851         r->left = v->left * n->width / d->width;
 852         r->top = v->top * n->height / d->height;
 853         r->width = v->width * n->width / d->width;
 854         r->height = v->height * n->height / d->height;
 855 }
 856
 857 static int s5k5baf_hw_set_crop_rects(struct s5k5baf *state)
 858 {
 859         struct v4l2_rect *p, r;
 860         u16 err;
 861         int ret;
 862
 863         p = &state->crop_sink;
 864         s5k5baf_write_seq(state, REG_G_PREVREQ_IN_WIDTH, p->width, p->height,
 865                           p->left, p->top);
 866
 867         s5k5baf_rescale(&r, &state->crop_source, &state->crop_sink,
 868                         &state->compose);
 869         s5k5baf_write_seq(state, REG_G_PREVZOOM_IN_WIDTH, r.width, r.height,
 870                           r.left, r.top);
 871
 872         s5k5baf_synchronize(state, 500, REG_G_INPUTS_CHANGE_REQ);
 873         s5k5baf_synchronize(state, 500, REG_G_PREV_CFG_BYPASS_CHANGED);
 874         err = s5k5baf_get_cfg_error(state);
 875         ret = s5k5baf_clear_error(state);
 876         if (ret < 0)
 877                 return ret;
 878
 879         switch (err) {
 880         case 0:
 881                 break;
 882         case CFG_ERROR_RANGE:
 883                 /* retry crop with frame interval set to max */
 884                 s5k5baf_hw_set_fiv(state, S5K5BAF_MAX_FR_TIME);
 885                 err = s5k5baf_get_cfg_error(state);
 886                 ret = s5k5baf_clear_error(state);
 887                 if (ret < 0)
 888                         return ret;
 889                 if (err) {
 890                         v4l2_err(&state->sd,
 891                                  "crop error on max frame interval: %d\n", err);
 892                         state->error = -EINVAL;
 893                 }
 894                 s5k5baf_hw_set_fiv(state, state->req_fiv);
 895                 s5k5baf_hw_validate_cfg(state);
 896                 break;
 897         default:
 898                 v4l2_err(&state->sd, "crop error: %d\n", err);
 899                 return -EINVAL;
 900         }
 901
 902         if (!state->apply_cfg)
 903                 return 0;
 904
 905         p = &state->crop_source;
 906         s5k5baf_write_seq(state, REG_P_OUT_WIDTH(0), p->width, p->height);
 907         s5k5baf_hw_set_fiv(state, state->req_fiv);
 908         s5k5baf_hw_validate_cfg(state);
 909
 910         return s5k5baf_clear_error(state);
 911 }
 912
 913 static void s5k5baf_hw_set_config(struct s5k5baf *state)
 914 {
 915         u16 reg_fmt = s5k5baf_formats[state->pixfmt].reg_p_fmt;
 916         struct v4l2_rect *r = &state->crop_source;
 917
 918         s5k5baf_write_seq(state, REG_P_OUT_WIDTH(0),
 919                           r->width, r->height, reg_fmt,
 920                           PCLK_MAX_FREQ >> 2, PCLK_MIN_FREQ >> 2,
 921                           PVI_MASK_MIPI, CLK_MIPI_INDEX,
 922                           FR_RATE_FIXED, FR_RATE_Q_DYNAMIC,
 923                           state->req_fiv, S5K5BAF_MIN_FR_TIME);
 924         s5k5baf_hw_sync_cfg(state);
 925         s5k5baf_hw_validate_cfg(state);
 926 }
 927
 928
 929 static void s5k5baf_hw_set_test_pattern(struct s5k5baf *state, int id)
 930 {
 931         s5k5baf_i2c_write(state, REG_PATTERN_WIDTH, 800);
 932         s5k5baf_i2c_write(state, REG_PATTERN_HEIGHT, 511);
 933         s5k5baf_i2c_write(state, REG_PATTERN_PARAM, 0);
 934         s5k5baf_i2c_write(state, REG_PATTERN_SET, id);
 935 }
 936
 937 static void s5k5baf_gpio_assert(struct s5k5baf *state, int id)
 938 {
 939         struct s5k5baf_gpio *gpio = &state->gpios[id];
 940
 941         gpio_set_value(gpio->gpio, gpio->level);
 942 }
 943
 944 static void s5k5baf_gpio_deassert(struct s5k5baf *state, int id)
 945 {
 946         struct s5k5baf_gpio *gpio = &state->gpios[id];
 947
 948         gpio_set_value(gpio->gpio, !gpio->level);
 949 }
 950
 951 static int s5k5baf_power_on(struct s5k5baf *state)
 952 {
 953         int ret;
 954
 955         ret = regulator_bulk_enable(S5K5BAF_NUM_SUPPLIES, state->supplies);
 956         if (ret < 0)
 957                 goto err;
 958
 959         ret = clk_set_rate(state->clock, state->mclk_frequency);
 960         if (ret < 0)
 961                 goto err_reg_dis;
 962
 963         ret = clk_prepare_enable(state->clock);
 964         if (ret < 0)
 965                 goto err_reg_dis;
 966
 967         v4l2_dbg(1, debug, &state->sd, "clock frequency: %ld\n",
 968                  clk_get_rate(state->clock));
 969
 970         s5k5baf_gpio_deassert(state, STBY);
 971         usleep_range(50, 100);
 972         s5k5baf_gpio_deassert(state, RST);
 973         return 0;
 974
 975 err_reg_dis:
 976         regulator_bulk_disable(S5K5BAF_NUM_SUPPLIES, state->supplies);
 977 err:
 978         v4l2_err(&state->sd, "%s() failed (%d)\n", __func__, ret);
 979         return ret;
 980 }
 981
 982 static int s5k5baf_power_off(struct s5k5baf *state)
 983 {
 984         int ret;
 985
 986         state->streaming = 0;
 987         state->apply_cfg = 0;
 988         state->apply_crop = 0;
 989
 990         s5k5baf_gpio_assert(state, RST);
 991         s5k5baf_gpio_assert(state, STBY);
 992
 993         if (!IS_ERR(state->clock))
 994                 clk_disable_unprepare(state->clock);
 995
 996         ret = regulator_bulk_disable(S5K5BAF_NUM_SUPPLIES,
 997                                         state->supplies);
 998         if (ret < 0)
 999                 v4l2_err(&state->sd, "failed to disable regulators\n");
1000
1001         return 0;
1002 }
1003
1004 static void s5k5baf_hw_init(struct s5k5baf *state)
1005 {
1006         s5k5baf_i2c_write(state, AHB_MSB_ADDR_PTR, PAGE_IF_HW);
1007         s5k5baf_i2c_write(state, REG_CLEAR_HOST_INT, 0);
1008         s5k5baf_i2c_write(state, REG_SW_LOAD_COMPLETE, 1);
1009         s5k5baf_i2c_write(state, REG_CMDRD_PAGE, PAGE_IF_SW);
1010         s5k5baf_i2c_write(state, REG_CMDWR_PAGE, PAGE_IF_SW);
1011 }
1012
1013 /*
1014  * V4L2 subdev core and video operations
1015  */
1016
1017 static void s5k5baf_initialize_data(struct s5k5baf *state)
1018 {
1019         state->pixfmt = 0;
1020         state->req_fiv = 10000 / 15;
1021         state->fiv = state->req_fiv;
1022         state->valid_auto_alg = 0;
1023 }
1024
1025 static int s5k5baf_load_setfile(struct s5k5baf *state)
1026 {
1027         struct i2c_client *c = v4l2_get_subdevdata(&state->sd);
1028         const struct firmware *fw;
1029         int ret;
1030
1031         ret = request_firmware(&fw, S5K5BAF_FW_FILENAME, &c->dev);
1032         if (ret < 0) {
1033                 dev_warn(&c->dev, "firmware file (%s) not loaded\n",
1034                          S5K5BAF_FW_FILENAME);
1035                 return ret;
1036         }
1037
1038         ret = s5k5baf_fw_parse(&c->dev, &state->fw, fw->size / 2,
1039                                (u16 *)fw->data);
1040
1041         release_firmware(fw);
1042
1043         return ret;
1044 }
1045
1046 static int s5k5baf_set_power(struct v4l2_subdev *sd, int on)
1047 {
1048         struct s5k5baf *state = to_s5k5baf(sd);
1049         int ret = 0;
1050
1051         mutex_lock(&state->lock);
1052
1053         if (!on != state->power)
1054                 goto out;
1055
1056         if (on) {
1057                 if (state->fw == NULL)
1058                         s5k5baf_load_setfile(state);
1059
1060                 s5k5baf_initialize_data(state);
1061                 ret = s5k5baf_power_on(state);
1062                 if (ret < 0)
1063                         goto out;
1064
1065                 s5k5baf_hw_init(state);
1066                 s5k5baf_hw_patch(state);
1067                 s5k5baf_i2c_write(state, REG_SET_HOST_INT, 1);
1068                 s5k5baf_hw_set_clocks(state);
1069
1070                 ret = s5k5baf_hw_set_video_bus(state);
1071                 if (ret < 0)
1072                         goto out;
1073
1074                 s5k5baf_hw_set_cis(state);
1075                 s5k5baf_hw_set_ccm(state);
1076
1077                 ret = s5k5baf_clear_error(state);
1078                 if (!ret)
1079                         state->power++;
1080         } else {
1081                 s5k5baf_power_off(state);
1082                 state->power--;
1083         }
1084
1085 out:
1086         mutex_unlock(&state->lock);
1087
1088         if (!ret && on)
1089                 ret = v4l2_ctrl_handler_setup(&state->ctrls.handler);
1090
1091         return ret;
1092 }
1093
1094 static void s5k5baf_hw_set_stream(struct s5k5baf *state, int enable)
1095 {
1096         s5k5baf_write_seq(state, REG_G_ENABLE_PREV, enable, 1);
1097 }
1098
1099 static int s5k5baf_s_stream(struct v4l2_subdev *sd, int on)
1100 {
1101         struct s5k5baf *state = to_s5k5baf(sd);
1102         int ret;
1103
1104         mutex_lock(&state->lock);
1105
1106         if (state->streaming == !!on) {
1107                 ret = 0;
1108                 goto out;
1109         }
1110
1111         if (on) {
1112                 s5k5baf_hw_set_config(state);
1113                 ret = s5k5baf_hw_set_crop_rects(state);
1114                 if (ret < 0)
1115                         goto out;
1116                 s5k5baf_hw_set_stream(state, 1);
1117                 s5k5baf_i2c_write(state, 0xb0cc, 0x000b);
1118         } else {
1119                 s5k5baf_hw_set_stream(state, 0);
1120         }
1121         ret = s5k5baf_clear_error(state);
1122         if (!ret)
1123                 state->streaming = !state->streaming;
1124
1125 out:
1126         mutex_unlock(&state->lock);
1127
1128         return ret;
1129 }
1130
1131 static int s5k5baf_g_frame_interval(struct v4l2_subdev *sd,
1132                                    struct v4l2_subdev_frame_interval *fi)
1133 {
1134         struct s5k5baf *state = to_s5k5baf(sd);
1135
1136         mutex_lock(&state->lock);
1137         fi->interval.numerator = state->fiv;
1138         fi->interval.denominator = 10000;
1139         mutex_unlock(&state->lock);
1140
1141         return 0;
1142 }
1143
1144 static void s5k5baf_set_frame_interval(struct s5k5baf *state,
1145                                        struct v4l2_subdev_frame_interval *fi)
1146 {
1147         struct v4l2_fract *i = &fi->interval;
1148
1149         if (fi->interval.denominator == 0)
1150                 state->req_fiv = S5K5BAF_MAX_FR_TIME;
1151         else
1152                 state->req_fiv = clamp_t(u32,
1153                                          i->numerator * 10000 / i->denominator,
1154                                          S5K5BAF_MIN_FR_TIME,
1155                                          S5K5BAF_MAX_FR_TIME);
1156
1157         state->fiv = state->req_fiv;
1158         if (state->apply_cfg) {
1159                 s5k5baf_hw_set_fiv(state, state->req_fiv);
1160                 s5k5baf_hw_validate_cfg(state);
1161         }
1162         *i = (struct v4l2_fract){ state->fiv, 10000 };
1163         if (state->fiv == state->req_fiv)
1164                 v4l2_info(&state->sd, "frame interval changed to %d00us\n",
1165                           state->fiv);
1166 }
1167
1168 static int s5k5baf_s_frame_interval(struct v4l2_subdev *sd,
1169                                    struct v4l2_subdev_frame_interval *fi)
1170 {
1171         struct s5k5baf *state = to_s5k5baf(sd);
1172
1173         mutex_lock(&state->lock);
1174         s5k5baf_set_frame_interval(state, fi);
1175         mutex_unlock(&state->lock);
1176         return 0;
1177 }
1178
1179 /*
1180  * V4L2 subdev pad level and video operations
1181  */
1182 static int s5k5baf_enum_frame_interval(struct v4l2_subdev *sd,
1183                               struct v4l2_subdev_fh *fh,
1184                               struct v4l2_subdev_frame_interval_enum *fie)
1185 {
1186         if (fie->index > S5K5BAF_MAX_FR_TIME - S5K5BAF_MIN_FR_TIME ||
1187             fie->pad != PAD_CIS)
1188                 return -EINVAL;
1189
1190         v4l_bound_align_image(&fie->width, S5K5BAF_WIN_WIDTH_MIN,
1191                               S5K5BAF_CIS_WIDTH, 1,
1192                               &fie->height, S5K5BAF_WIN_HEIGHT_MIN,
1193                               S5K5BAF_CIS_HEIGHT, 1, 0);
1194
1195         fie->interval.numerator = S5K5BAF_MIN_FR_TIME + fie->index;
1196         fie->interval.denominator = 10000;
1197
1198         return 0;
1199 }
1200
1201 static int s5k5baf_enum_mbus_code(struct v4l2_subdev *sd,
1202                                  struct v4l2_subdev_fh *fh,
1203                                  struct v4l2_subdev_mbus_code_enum *code)
1204 {
1205         if (code->pad == PAD_CIS) {
1206                 if (code->index > 0)
1207                         return -EINVAL;
1208                 code->code = V4L2_MBUS_FMT_FIXED;
1209                 return 0;
1210         }
1211
1212         if (code->index >= ARRAY_SIZE(s5k5baf_formats))
1213                 return -EINVAL;
1214
1215         code->code = s5k5baf_formats[code->index].code;
1216         return 0;
1217 }
1218
1219 static int s5k5baf_enum_frame_size(struct v4l2_subdev *sd,
1220                                   struct v4l2_subdev_fh *fh,
1221                                   struct v4l2_subdev_frame_size_enum *fse)
1222 {
1223         int i;
1224
1225         if (fse->index > 0)
1226                 return -EINVAL;
1227
1228         if (fse->pad == PAD_CIS) {
1229                 fse->code = V4L2_MBUS_FMT_FIXED;
1230                 fse->min_width = S5K5BAF_CIS_WIDTH;
1231                 fse->max_width = S5K5BAF_CIS_WIDTH;
1232                 fse->min_height = S5K5BAF_CIS_HEIGHT;
1233                 fse->max_height = S5K5BAF_CIS_HEIGHT;
1234                 return 0;
1235         }
1236
1237         i = ARRAY_SIZE(s5k5baf_formats);
1238         while (--i)
1239                 if (fse->code == s5k5baf_formats[i].code)
1240                         break;
1241         fse->code = s5k5baf_formats[i].code;
1242         fse->min_width = S5K5BAF_WIN_WIDTH_MIN;
1243         fse->max_width = S5K5BAF_CIS_WIDTH;
1244         fse->max_height = S5K5BAF_WIN_HEIGHT_MIN;
1245         fse->min_height = S5K5BAF_CIS_HEIGHT;
1246
1247         return 0;
1248 }
1249
1250 static void s5k5baf_try_cis_format(struct v4l2_mbus_framefmt *mf)
1251 {
1252         mf->width = S5K5BAF_CIS_WIDTH;
1253         mf->height = S5K5BAF_CIS_HEIGHT;
1254         mf->code = V4L2_MBUS_FMT_FIXED;
1255         mf->colorspace = V4L2_COLORSPACE_JPEG;
1256         mf->field = V4L2_FIELD_NONE;
1257 }
1258
1259 static int s5k5baf_try_isp_format(struct v4l2_mbus_framefmt *mf)
1260 {
1261         int pixfmt;
1262
1263         v4l_bound_align_image(&mf->width, S5K5BAF_WIN_WIDTH_MIN,
1264                               S5K5BAF_CIS_WIDTH, 1,
1265                               &mf->height, S5K5BAF_WIN_HEIGHT_MIN,
1266                               S5K5BAF_CIS_HEIGHT, 1, 0);
1267
1268         pixfmt = s5k5baf_find_pixfmt(mf);
1269
1270         mf->colorspace = s5k5baf_formats[pixfmt].colorspace;
1271         mf->code = s5k5baf_formats[pixfmt].code;
1272         mf->field = V4L2_FIELD_NONE;
1273
1274         return pixfmt;
1275 }
1276
1277 static int s5k5baf_get_fmt(struct v4l2_subdev *sd, struct v4l2_subdev_fh *fh,
1278                           struct v4l2_subdev_format *fmt)
1279 {
1280         struct s5k5baf *state = to_s5k5baf(sd);
1281         const struct s5k5baf_pixfmt *pixfmt;
1282         struct v4l2_mbus_framefmt *mf;
1283
1284         if (fmt->which == V4L2_SUBDEV_FORMAT_TRY) {
1285                 mf = v4l2_subdev_get_try_format(fh, fmt->pad);
1286                 fmt->format = *mf;
1287                 return 0;
1288         }
1289
1290         mf = &fmt->format;
1291         if (fmt->pad == PAD_CIS) {
1292                 s5k5baf_try_cis_format(mf);
1293                 return 0;
1294         }
1295         mf->field = V4L2_FIELD_NONE;
1296         mutex_lock(&state->lock);
1297         pixfmt = &s5k5baf_formats[state->pixfmt];
1298         mf->width = state->crop_source.width;
1299         mf->height = state->crop_source.height;
1300         mf->code = pixfmt->code;
1301         mf->colorspace = pixfmt->colorspace;
1302         mutex_unlock(&state->lock);
1303
1304         return 0;
1305 }
1306
1307 static int s5k5baf_set_fmt(struct v4l2_subdev *sd, struct v4l2_subdev_fh *fh,
1308                           struct v4l2_subdev_format *fmt)
1309 {
1310         struct v4l2_mbus_framefmt *mf = &fmt->format;
1311         struct s5k5baf *state = to_s5k5baf(sd);
1312         const struct s5k5baf_pixfmt *pixfmt;
1313         int ret = 0;
1314
1315         if (fmt->which == V4L2_SUBDEV_FORMAT_TRY) {
1316                 *v4l2_subdev_get_try_format(fh, fmt->pad) = *mf;
1317                 return 0;
1318         }
1319
1320         if (fmt->pad == PAD_CIS) {
1321                 s5k5baf_try_cis_format(mf);
1322                 return 0;
1323         }
1324
1325         mutex_lock(&state->lock);
1326
1327         if (state->streaming) {
1328                 mutex_unlock(&state->lock);
1329                 return -EBUSY;
1330         }
1331
1332         state->pixfmt = s5k5baf_try_isp_format(mf);
1333         pixfmt = &s5k5baf_formats[state->pixfmt];
1334         mf->code = pixfmt->code;
1335         mf->colorspace = pixfmt->colorspace;
1336         mf->width = state->crop_source.width;
1337         mf->height = state->crop_source.height;
1338
1339         mutex_unlock(&state->lock);
1340         return ret;
1341 }
1342
1343 enum selection_rect { R_CIS, R_CROP_SINK, R_COMPOSE, R_CROP_SOURCE, R_INVALID };
1344
1345 static enum selection_rect s5k5baf_get_sel_rect(u32 pad, u32 target)
1346 {
1347         switch (target) {
1348         case V4L2_SEL_TGT_CROP_BOUNDS:
1349                 return pad ? R_COMPOSE : R_CIS;
1350         case V4L2_SEL_TGT_CROP:
1351                 return pad ? R_CROP_SOURCE : R_CROP_SINK;
1352         case V4L2_SEL_TGT_COMPOSE_BOUNDS:
1353                 return pad ? R_INVALID : R_CROP_SINK;
1354         case V4L2_SEL_TGT_COMPOSE:
1355                 return pad ? R_INVALID : R_COMPOSE;
1356         default:
1357                 return R_INVALID;
1358         }
1359 }
1360
1361 static int s5k5baf_is_bound_target(u32 target)
1362 {
1363         return target == V4L2_SEL_TGT_CROP_BOUNDS ||
1364                 target == V4L2_SEL_TGT_COMPOSE_BOUNDS;
1365 }
1366
1367 static int s5k5baf_get_selection(struct v4l2_subdev *sd,
1368                                  struct v4l2_subdev_fh *fh,
1369                                  struct v4l2_subdev_selection *sel)
1370 {
1371         static enum selection_rect rtype;
1372         struct s5k5baf *state = to_s5k5baf(sd);
1373
1374         rtype = s5k5baf_get_sel_rect(sel->pad, sel->target);
1375
1376         switch (rtype) {
1377         case R_INVALID:
1378                 return -EINVAL;
1379         case R_CIS:
1380                 sel->r = s5k5baf_cis_rect;
1381                 return 0;
1382         default:
1383                 break;
1384         }
1385
1386         if (sel->which == V4L2_SUBDEV_FORMAT_TRY) {
1387                 if (rtype == R_COMPOSE)
1388                         sel->r = *v4l2_subdev_get_try_compose(fh, sel->pad);
1389                 else
1390                         sel->r = *v4l2_subdev_get_try_crop(fh, sel->pad);
1391                 return 0;
1392         }
1393
1394         mutex_lock(&state->lock);
1395         switch (rtype) {
1396         case R_CROP_SINK:
1397                 sel->r = state->crop_sink;
1398                 break;
1399         case R_COMPOSE:
1400                 sel->r = state->compose;
1401                 break;
1402         case R_CROP_SOURCE:
1403                 sel->r = state->crop_source;
1404                 break;
1405         default:
1406                 break;
1407         }
1408         if (s5k5baf_is_bound_target(sel->target)) {
1409                 sel->r.left = 0;
1410                 sel->r.top = 0;
1411         }
1412         mutex_unlock(&state->lock);
1413
1414         return 0;
1415 }
1416
1417 /* bounds range [start, start+len) to [0, max) and aligns to 2 */
1418 static void s5k5baf_bound_range(u32 *start, u32 *len, u32 max)
1419 {
1420         if (*len > max)
1421                 *len = max;
1422         if (*start + *len > max)
1423                 *start = max - *len;
1424         *start &= ~1;
1425         *len &= ~1;
1426         if (*len < S5K5BAF_WIN_WIDTH_MIN)
1427                 *len = S5K5BAF_WIN_WIDTH_MIN;
1428 }
1429
1430 static void s5k5baf_bound_rect(struct v4l2_rect *r, u32 width, u32 height)
1431 {
1432         s5k5baf_bound_range(&r->left, &r->width, width);
1433         s5k5baf_bound_range(&r->top, &r->height, height);
1434 }
1435
1436 static void s5k5baf_set_rect_and_adjust(struct v4l2_rect **rects,
1437                                         enum selection_rect first,
1438                                         struct v4l2_rect *v)
1439 {
1440         struct v4l2_rect *r, *br;
1441         enum selection_rect i = first;
1442
1443         *rects[first] = *v;
1444         do {
1445                 r = rects[i];
1446                 br = rects[i - 1];
1447                 s5k5baf_bound_rect(r, br->width, br->height);
1448         } while (++i != R_INVALID);
1449         *v = *rects[first];
1450 }
1451
1452 static bool s5k5baf_cmp_rect(const struct v4l2_rect *r1,
1453                              const struct v4l2_rect *r2)
1454 {
1455         return !memcmp(r1, r2, sizeof(*r1));
1456 }
1457
1458 static int s5k5baf_set_selection(struct v4l2_subdev *sd,
1459                                  struct v4l2_subdev_fh *fh,
1460                                  struct v4l2_subdev_selection *sel)
1461 {
1462         static enum selection_rect rtype;
1463         struct s5k5baf *state = to_s5k5baf(sd);
1464         struct v4l2_rect **rects;
1465         int ret = 0;
1466
1467         rtype = s5k5baf_get_sel_rect(sel->pad, sel->target);
1468         if (rtype == R_INVALID || s5k5baf_is_bound_target(sel->target))
1469                 return -EINVAL;
1470
1471         /* allow only scaling on compose */
1472         if (rtype == R_COMPOSE) {
1473                 sel->r.left = 0;
1474                 sel->r.top = 0;
1475         }
1476
1477         if (sel->which == V4L2_SUBDEV_FORMAT_TRY) {
1478                 rects = (struct v4l2_rect * []) {
1479                                 &s5k5baf_cis_rect,
1480                                 v4l2_subdev_get_try_crop(fh, PAD_CIS),
1481                                 v4l2_subdev_get_try_compose(fh, PAD_CIS),
1482                                 v4l2_subdev_get_try_crop(fh, PAD_OUT)
1483                         };
1484                 s5k5baf_set_rect_and_adjust(rects, rtype, &sel->r);
1485                 return 0;
1486         }
1487
1488         rects = (struct v4l2_rect * []) {
1489                         &s5k5baf_cis_rect,
1490                         &state->crop_sink,
1491                         &state->compose,
1492                         &state->crop_source
1493                 };
1494         mutex_lock(&state->lock);
1495         if (state->streaming) {
1496                 /* adjust sel->r to avoid output resolution change */
1497                 if (rtype < R_CROP_SOURCE) {
1498                         if (sel->r.width < state->crop_source.width)
1499                                 sel->r.width = state->crop_source.width;
1500                         if (sel->r.height < state->crop_source.height)
1501                                 sel->r.height = state->crop_source.height;
1502                 } else {
1503                         sel->r.width = state->crop_source.width;
1504                         sel->r.height = state->crop_source.height;
1505                 }
1506         }
1507         s5k5baf_set_rect_and_adjust(rects, rtype, &sel->r);
1508         if (!s5k5baf_cmp_rect(&state->crop_sink, &s5k5baf_cis_rect) ||
1509             !s5k5baf_cmp_rect(&state->compose, &s5k5baf_cis_rect))
1510                 state->apply_crop = 1;
1511         if (state->streaming)
1512                 ret = s5k5baf_hw_set_crop_rects(state);
1513         mutex_unlock(&state->lock);
1514
1515         return ret;
1516 }
1517
1518 static const struct v4l2_subdev_pad_ops s5k5baf_cis_pad_ops = {
1519         .enum_mbus_code         = s5k5baf_enum_mbus_code,
1520         .enum_frame_size        = s5k5baf_enum_frame_size,
1521         .get_fmt                = s5k5baf_get_fmt,
1522         .set_fmt                = s5k5baf_set_fmt,
1523 };
1524
1525 static const struct v4l2_subdev_pad_ops s5k5baf_pad_ops = {
1526         .enum_mbus_code         = s5k5baf_enum_mbus_code,
1527         .enum_frame_size        = s5k5baf_enum_frame_size,
1528         .enum_frame_interval    = s5k5baf_enum_frame_interval,
1529         .get_fmt                = s5k5baf_get_fmt,
1530         .set_fmt                = s5k5baf_set_fmt,
1531         .get_selection          = s5k5baf_get_selection,
1532         .set_selection          = s5k5baf_set_selection,
1533 };
1534
1535 static const struct v4l2_subdev_video_ops s5k5baf_video_ops = {
1536         .g_frame_interval       = s5k5baf_g_frame_interval,
1537         .s_frame_interval       = s5k5baf_s_frame_interval,
1538         .s_stream               = s5k5baf_s_stream,
1539 };
1540
1541 /*
1542  * V4L2 subdev controls
1543  */
1544
1545 static int s5k5baf_s_ctrl(struct v4l2_ctrl *ctrl)
1546 {
1547         struct v4l2_subdev *sd = ctrl_to_sd(ctrl);
1548         struct s5k5baf *state = to_s5k5baf(sd);
1549         int ret;
1550
1551         v4l2_dbg(1, debug, sd, "ctrl: %s, value: %d\n", ctrl->name, ctrl->val);
1552
1553         mutex_lock(&state->lock);
1554
1555         if (state->power == 0)
1556                 goto unlock;
1557
1558         switch (ctrl->id) {
1559         case V4L2_CID_AUTO_WHITE_BALANCE:
1560                 s5k5baf_hw_set_awb(state, ctrl->val);
1561                 break;
1562
1563         case V4L2_CID_BRIGHTNESS:
1564                 s5k5baf_write(state, REG_USER_BRIGHTNESS, ctrl->val);
1565                 break;
1566
1567         case V4L2_CID_COLORFX:
1568                 s5k5baf_hw_set_colorfx(state, ctrl->val);
1569                 break;
1570
1571         case V4L2_CID_CONTRAST:
1572                 s5k5baf_write(state, REG_USER_CONTRAST, ctrl->val);
1573                 break;
1574
1575         case V4L2_CID_EXPOSURE_AUTO:
1576                 s5k5baf_hw_set_auto_exposure(state, ctrl->val);
1577                 break;
1578
1579         case V4L2_CID_HFLIP:
1580                 s5k5baf_hw_set_mirror(state);
1581                 break;
1582
1583         case V4L2_CID_POWER_LINE_FREQUENCY:
1584                 s5k5baf_hw_set_anti_flicker(state, ctrl->val);
1585                 break;
1586
1587         case V4L2_CID_SATURATION:
1588                 s5k5baf_write(state, REG_USER_SATURATION, ctrl->val);
1589                 break;
1590
1591         case V4L2_CID_SHARPNESS:
1592                 s5k5baf_write(state, REG_USER_SHARPBLUR, ctrl->val);
1593                 break;
1594
1595         case V4L2_CID_WHITE_BALANCE_TEMPERATURE:
1596                 s5k5baf_write(state, REG_P_COLORTEMP(0), ctrl->val);
1597                 if (state->apply_cfg)
1598                         s5k5baf_hw_sync_cfg(state);
1599                 break;
1600
1601         case V4L2_CID_TEST_PATTERN:
1602                 s5k5baf_hw_set_test_pattern(state, ctrl->val);
1603                 break;
1604         }
1605 unlock:
1606         ret = s5k5baf_clear_error(state);
1607         mutex_unlock(&state->lock);
1608         return ret;
1609 }
1610
1611 static const struct v4l2_ctrl_ops s5k5baf_ctrl_ops = {
1612         .s_ctrl = s5k5baf_s_ctrl,
1613 };
1614
1615 static const char * const s5k5baf_test_pattern_menu[] = {
1616         "Disabled",
1617         "Blank",
1618         "Bars",
1619         "Gradients",
1620         "Textile",
1621         "Textile2",
1622         "Squares"
1623 };
1624
1625 static int s5k5baf_initialize_ctrls(struct s5k5baf *state)
1626 {
1627         const struct v4l2_ctrl_ops *ops = &s5k5baf_ctrl_ops;
1628         struct s5k5baf_ctrls *ctrls = &state->ctrls;
1629         struct v4l2_ctrl_handler *hdl = &ctrls->handler;
1630         int ret;
1631
1632         ret = v4l2_ctrl_handler_init(hdl, 16);
1633         if (ret < 0) {
1634                 v4l2_err(&state->sd, "cannot init ctrl handler (%d)\n", ret);
1635                 return ret;
1636         }
1637
1638         /* Auto white balance cluster */
1639         ctrls->awb = v4l2_ctrl_new_std(hdl, ops, V4L2_CID_AUTO_WHITE_BALANCE,
1640                                        0, 1, 1, 1);
1641         ctrls->gain_red = v4l2_ctrl_new_std(hdl, ops, V4L2_CID_RED_BALANCE,
1642                                             0, 255, 1, S5K5BAF_GAIN_RED_DEF);
1643         ctrls->gain_blue = v4l2_ctrl_new_std(hdl, ops, V4L2_CID_BLUE_BALANCE,
1644                                              0, 255, 1, S5K5BAF_GAIN_BLUE_DEF);
1645         v4l2_ctrl_auto_cluster(3, &ctrls->awb, 0, false);
1646
1647         ctrls->hflip = v4l2_ctrl_new_std(hdl, ops, V4L2_CID_HFLIP, 0, 1, 1, 0);
1648         ctrls->vflip = v4l2_ctrl_new_std(hdl, ops, V4L2_CID_VFLIP, 0, 1, 1, 0);
1649         v4l2_ctrl_cluster(2, &ctrls->hflip);
1650
1651         ctrls->auto_exp = v4l2_ctrl_new_std_menu(hdl, ops,
1652                                 V4L2_CID_EXPOSURE_AUTO,
1653                                 V4L2_EXPOSURE_MANUAL, 0, V4L2_EXPOSURE_AUTO);
1654         /* Exposure time: x 1 us */
1655         ctrls->exposure = v4l2_ctrl_new_std(hdl, ops, V4L2_CID_EXPOSURE,
1656                                             0, 6000000U, 1, 100000U);
1657         /* Total gain: 256 <=> 1x */
1658         ctrls->gain = v4l2_ctrl_new_std(hdl, ops, V4L2_CID_GAIN,
1659                                         0, 256, 1, 256);
1660         v4l2_ctrl_auto_cluster(3, &ctrls->auto_exp, 0, false);
1661
1662         v4l2_ctrl_new_std_menu(hdl, ops, V4L2_CID_POWER_LINE_FREQUENCY,
1663                                V4L2_CID_POWER_LINE_FREQUENCY_AUTO, 0,
1664                                V4L2_CID_POWER_LINE_FREQUENCY_AUTO);
1665
1666         v4l2_ctrl_new_std_menu(hdl, ops, V4L2_CID_COLORFX,
1667                                V4L2_COLORFX_SKY_BLUE, ~0x6f, V4L2_COLORFX_NONE);
1668
1669         v4l2_ctrl_new_std(hdl, ops, V4L2_CID_WHITE_BALANCE_TEMPERATURE,
1670                           0, 256, 1, 0);
1671
1672         v4l2_ctrl_new_std(hdl, ops, V4L2_CID_SATURATION, -127, 127, 1, 0);
1673         v4l2_ctrl_new_std(hdl, ops, V4L2_CID_BRIGHTNESS, -127, 127, 1, 0);
1674         v4l2_ctrl_new_std(hdl, ops, V4L2_CID_CONTRAST, -127, 127, 1, 0);
1675         v4l2_ctrl_new_std(hdl, ops, V4L2_CID_SHARPNESS, -127, 127, 1, 0);
1676
1677         v4l2_ctrl_new_std_menu_items(hdl, ops, V4L2_CID_TEST_PATTERN,
1678                                      ARRAY_SIZE(s5k5baf_test_pattern_menu) - 1,
1679                                      0, 0, s5k5baf_test_pattern_menu);
1680
1681         if (hdl->error) {
1682                 v4l2_err(&state->sd, "error creating controls (%d)\n",
1683                          hdl->error);
1684                 ret = hdl->error;
1685                 v4l2_ctrl_handler_free(hdl);
1686                 return ret;
1687         }
1688
1689         state->sd.ctrl_handler = hdl;
1690         return 0;
1691 }
1692
1693 /*
1694  * V4L2 subdev internal operations
1695  */
1696 static int s5k5baf_open(struct v4l2_subdev *sd, struct v4l2_subdev_fh *fh)
1697 {
1698         struct v4l2_mbus_framefmt *mf;
1699
1700         mf = v4l2_subdev_get_try_format(fh, PAD_CIS);
1701         s5k5baf_try_cis_format(mf);
1702
1703         if (s5k5baf_is_cis_subdev(sd))
1704                 return 0;
1705
1706         mf = v4l2_subdev_get_try_format(fh, PAD_OUT);
1707         mf->colorspace = s5k5baf_formats[0].colorspace;
1708         mf->code = s5k5baf_formats[0].code;
1709         mf->width = s5k5baf_cis_rect.width;
1710         mf->height = s5k5baf_cis_rect.height;
1711         mf->field = V4L2_FIELD_NONE;
1712
1713         *v4l2_subdev_get_try_crop(fh, PAD_CIS) = s5k5baf_cis_rect;
1714         *v4l2_subdev_get_try_compose(fh, PAD_CIS) = s5k5baf_cis_rect;
1715         *v4l2_subdev_get_try_crop(fh, PAD_OUT) = s5k5baf_cis_rect;
1716
1717         return 0;
1718 }
1719
1720 static int s5k5baf_check_fw_revision(struct s5k5baf *state)
1721 {
1722         u16 api_ver = 0, fw_rev = 0, s_id = 0;
1723         int ret;
1724
1725         api_ver = s5k5baf_read(state, REG_FW_APIVER);
1726         fw_rev = s5k5baf_read(state, REG_FW_REVISION) & 0xff;
1727         s_id = s5k5baf_read(state, REG_FW_SENSOR_ID);
1728         ret = s5k5baf_clear_error(state);
1729         if (ret < 0)
1730                 return ret;
1731
1732         v4l2_info(&state->sd, "FW API=%#x, revision=%#x sensor_id=%#x\n",
1733                   api_ver, fw_rev, s_id);
1734
1735         if (api_ver != S5K5BAF_FW_APIVER) {
1736                 v4l2_err(&state->sd, "FW API version not supported\n");
1737                 return -ENODEV;
1738         }
1739
1740         return 0;
1741 }
1742
1743 static int s5k5baf_registered(struct v4l2_subdev *sd)
1744 {
1745         struct s5k5baf *state = to_s5k5baf(sd);
1746         int ret;
1747
1748         ret = v4l2_device_register_subdev(sd->v4l2_dev, &state->cis_sd);
1749         if (ret < 0)
1750                 v4l2_err(sd, "failed to register subdev %s\n",
1751                          state->cis_sd.name);
1752         else
1753                 ret = media_entity_create_link(&state->cis_sd.entity, PAD_CIS,
1754                                                &state->sd.entity, PAD_CIS,
1755                                                MEDIA_LNK_FL_IMMUTABLE |
1756                                                MEDIA_LNK_FL_ENABLED);
1757         return ret;
1758 }
1759
1760 static void s5k5baf_unregistered(struct v4l2_subdev *sd)
1761 {
1762         struct s5k5baf *state = to_s5k5baf(sd);
1763         v4l2_device_unregister_subdev(&state->cis_sd);
1764 }
1765
1766 static const struct v4l2_subdev_ops s5k5baf_cis_subdev_ops = {
1767         .pad    = &s5k5baf_cis_pad_ops,
1768 };
1769
1770 static const struct v4l2_subdev_internal_ops s5k5baf_cis_subdev_internal_ops = {
1771         .open = s5k5baf_open,
1772 };
1773
1774 static const struct v4l2_subdev_internal_ops s5k5baf_subdev_internal_ops = {
1775         .registered = s5k5baf_registered,
1776         .unregistered = s5k5baf_unregistered,
1777         .open = s5k5baf_open,
1778 };
1779
1780 static const struct v4l2_subdev_core_ops s5k5baf_core_ops = {
1781         .s_power = s5k5baf_set_power,
1782         .log_status = v4l2_ctrl_subdev_log_status,
1783 };
1784
1785 static const struct v4l2_subdev_ops s5k5baf_subdev_ops = {
1786         .core = &s5k5baf_core_ops,
1787         .pad = &s5k5baf_pad_ops,
1788         .video = &s5k5baf_video_ops,
1789 };
1790
1791 static int s5k5baf_configure_gpios(struct s5k5baf *state)
1792 {
1793         static const char const *name[] = { "S5K5BAF_STBY", "S5K5BAF_RST" };
1794         struct i2c_client *c = v4l2_get_subdevdata(&state->sd);
1795         struct s5k5baf_gpio *g = state->gpios;
1796         int ret, i;
1797
1798         for (i = 0; i < NUM_GPIOS; ++i) {
1799                 int flags = GPIOF_DIR_OUT;
1800                 if (g[i].level)
1801                         flags |= GPIOF_INIT_HIGH;
1802                 ret = devm_gpio_request_one(&c->dev, g[i].gpio, flags, name[i]);
1803                 if (ret < 0) {
1804                         v4l2_err(c, "failed to request gpio %s\n", name[i]);
1805                         return ret;
1806                 }
1807         }
1808         return 0;
1809 }
1810
1811 static int s5k5baf_parse_gpios(struct s5k5baf_gpio *gpios, struct device *dev)
1812 {
1813         static const char * const names[] = {
1814                 "stbyn-gpios",
1815                 "rstn-gpios",
1816         };
1817         struct device_node *node = dev->of_node;
1818         enum of_gpio_flags flags;
1819         int ret, i;
1820
1821         for (i = 0; i < NUM_GPIOS; ++i) {
1822                 ret = of_get_named_gpio_flags(node, names[i], 0, &flags);
1823                 if (ret < 0) {
1824                         dev_err(dev, "no %s GPIO pin provided\n", names[i]);
1825                         return ret;
1826                 }
1827                 gpios[i].gpio = ret;
1828                 gpios[i].level = !(flags & OF_GPIO_ACTIVE_LOW);
1829         }
1830
1831         return 0;
1832 }
1833
1834 static int s5k5baf_parse_device_node(struct s5k5baf *state, struct device *dev)
1835 {
1836         struct device_node *node = dev->of_node;
1837         struct device_node *node_ep;
1838         struct v4l2_of_endpoint ep;
1839         int ret;
1840
1841         if (!node) {
1842                 dev_err(dev, "no device-tree node provided\n");
1843                 return -EINVAL;
1844         }
1845
1846         ret = of_property_read_u32(node, "clock-frequency",
1847                                    &state->mclk_frequency);
1848         if (ret < 0) {
1849                 state->mclk_frequency = S5K5BAF_DEFAULT_MCLK_FREQ;
1850                 dev_info(dev, "using default %u Hz clock frequency\n",
1851                          state->mclk_frequency);
1852         }
1853
1854         ret = s5k5baf_parse_gpios(state->gpios, dev);
1855         if (ret < 0)
1856                 return ret;
1857
1858         node_ep = v4l2_of_get_next_endpoint(node, NULL);
1859         if (!node_ep) {
1860                 dev_err(dev, "no endpoint defined at node %s\n",
1861                         node->full_name);
1862                 return -EINVAL;
1863         }
1864
1865         v4l2_of_parse_endpoint(node_ep, &ep);
1866         of_node_put(node_ep);
1867         state->bus_type = ep.bus_type;
1868
1869         switch (state->bus_type) {
1870         case V4L2_MBUS_CSI2:
1871                 state->nlanes = ep.bus.mipi_csi2.num_data_lanes;
1872                 break;
1873         case V4L2_MBUS_PARALLEL:
1874                 break;
1875         default:
1876                 dev_err(dev, "unsupported bus in endpoint defined at node %s\n",
1877                         node->full_name);
1878                 return -EINVAL;
1879         }
1880
1881         return 0;
1882 }
1883
1884 static int s5k5baf_configure_subdevs(struct s5k5baf *state,
1885                                      struct i2c_client *c)
1886 {
1887         struct v4l2_subdev *sd;
1888         int ret;
1889
1890         sd = &state->cis_sd;
1891         v4l2_subdev_init(sd, &s5k5baf_cis_subdev_ops);
1892         sd->owner = THIS_MODULE;
1893         v4l2_set_subdevdata(sd, state);
1894         snprintf(sd->name, sizeof(sd->name), "S5K5BAF-CIS %d-%04x",
1895                  i2c_adapter_id(c->adapter), c->addr);
1896
1897         sd->internal_ops = &s5k5baf_cis_subdev_internal_ops;
1898         sd->flags |= V4L2_SUBDEV_FL_HAS_DEVNODE;
1899
1900         state->cis_pad.flags = MEDIA_PAD_FL_SOURCE;
1901         sd->entity.type = MEDIA_ENT_T_V4L2_SUBDEV_SENSOR;
1902         ret = media_entity_init(&sd->entity, NUM_CIS_PADS, &state->cis_pad, 0);
1903         if (ret < 0)
1904                 goto err;
1905
1906         sd = &state->sd;
1907         v4l2_i2c_subdev_init(sd, c, &s5k5baf_subdev_ops);
1908         snprintf(sd->name, sizeof(sd->name), "S5K5BAF-ISP %d-%04x",
1909                  i2c_adapter_id(c->adapter), c->addr);
1910
1911         sd->internal_ops = &s5k5baf_subdev_internal_ops;
1912         sd->flags |= V4L2_SUBDEV_FL_HAS_DEVNODE;
1913
1914         state->pads[PAD_CIS].flags = MEDIA_PAD_FL_SINK;
1915         state->pads[PAD_OUT].flags = MEDIA_PAD_FL_SOURCE;
1916         sd->entity.type = MEDIA_ENT_T_V4L2_SUBDEV;
1917         ret = media_entity_init(&sd->entity, NUM_ISP_PADS, state->pads, 0);
1918
1919         if (!ret)
1920                 return 0;
1921
1922         media_entity_cleanup(&state->cis_sd.entity);
1923 err:
1924         dev_err(&c->dev, "cannot init media entity %s\n", sd->name);
1925         return ret;
1926 }
1927
1928 static int s5k5baf_configure_regulators(struct s5k5baf *state)
1929 {
1930         struct i2c_client *c = v4l2_get_subdevdata(&state->sd);
1931         int ret;
1932         int i;
1933
1934         for (i = 0; i < S5K5BAF_NUM_SUPPLIES; i++)
1935                 state->supplies[i].supply = s5k5baf_supply_names[i];
1936
1937         ret = devm_regulator_bulk_get(&c->dev, S5K5BAF_NUM_SUPPLIES,
1938                                       state->supplies);
1939         if (ret < 0)
1940                 v4l2_err(c, "failed to get regulators\n");
1941         return ret;
1942 }
1943
1944 static int s5k5baf_probe(struct i2c_client *c,
1945                         const struct i2c_device_id *id)
1946 {
1947         struct s5k5baf *state;
1948         int ret;
1949
1950         state = devm_kzalloc(&c->dev, sizeof(*state), GFP_KERNEL);
1951         if (!state)
1952                 return -ENOMEM;
1953
1954         mutex_init(&state->lock);
1955         state->crop_sink = s5k5baf_cis_rect;
1956         state->compose = s5k5baf_cis_rect;
1957         state->crop_source = s5k5baf_cis_rect;
1958
1959         ret = s5k5baf_parse_device_node(state, &c->dev);
1960         if (ret < 0)
1961                 return ret;
1962
1963         ret = s5k5baf_configure_subdevs(state, c);
1964         if (ret < 0)
1965                 return ret;
1966
1967         ret = s5k5baf_configure_gpios(state);
1968         if (ret < 0)
1969                 goto err_me;
1970
1971         ret = s5k5baf_configure_regulators(state);
1972         if (ret < 0)
1973                 goto err_me;
1974
1975         state->clock = devm_clk_get(state->sd.dev, S5K5BAF_CLK_NAME);
1976         if (IS_ERR(state->clock)) {
1977                 ret = -EPROBE_DEFER;
1978                 goto err_me;
1979         }
1980
1981         ret = s5k5baf_power_on(state);
1982         if (ret < 0) {
1983                 ret = -EPROBE_DEFER;
1984                 goto err_me;
1985         }
1986         s5k5baf_hw_init(state);
1987         ret = s5k5baf_check_fw_revision(state);
1988
1989         s5k5baf_power_off(state);
1990         if (ret < 0)
1991                 goto err_me;
1992
1993         ret = s5k5baf_initialize_ctrls(state);
1994         if (ret < 0)
1995                 goto err_me;
1996
1997         ret = v4l2_async_register_subdev(&state->sd);
1998         if (ret < 0)
1999                 goto err_ctrl;
2000
2001         return 0;
2002
2003 err_ctrl:
2004         v4l2_ctrl_handler_free(state->sd.ctrl_handler);
2005 err_me:
2006         media_entity_cleanup(&state->sd.entity);
2007         media_entity_cleanup(&state->cis_sd.entity);
2008         return ret;
2009 }
2010
2011 static int s5k5baf_remove(struct i2c_client *c)
2012 {
2013         struct v4l2_subdev *sd = i2c_get_clientdata(c);
2014         struct s5k5baf *state = to_s5k5baf(sd);
2015
2016         v4l2_async_unregister_subdev(sd);
2017         v4l2_ctrl_handler_free(sd->ctrl_handler);
2018         media_entity_cleanup(&sd->entity);
2019
2020         sd = &state->cis_sd;
2021         v4l2_device_unregister_subdev(sd);
2022         media_entity_cleanup(&sd->entity);
2023
2024         return 0;
2025 }
2026
2027 static const struct i2c_device_id s5k5baf_id[] = {
2028         { S5K5BAF_DRIVER_NAME, 0 },
2029         { },
2030 };
2031 MODULE_DEVICE_TABLE(i2c, s5k5baf_id);
2032
2033 static const struct of_device_id s5k5baf_of_match[] = {
2034         { .compatible = "samsung,s5k5baf" },
2035         { }
2036 };
2037 MODULE_DEVICE_TABLE(of, s5k5baf_of_match);
2038
2039 static struct i2c_driver s5k5baf_i2c_driver = {
2040         .driver = {
2041                 .of_match_table = s5k5baf_of_match,
2042                 .name = S5K5BAF_DRIVER_NAME
2043         },
2044         .probe          = s5k5baf_probe,
2045         .remove         = s5k5baf_remove,
2046         .id_table       = s5k5baf_id,
2047 };
2048
2049 module_i2c_driver(s5k5baf_i2c_driver);
2050
2051 MODULE_DESCRIPTION("Samsung S5K5BAF(X) UXGA camera driver");
2052 MODULE_AUTHOR("Andrzej Hajda <a.hajda@samsung.com>");
2053 MODULE_LICENSE("GPL v2");