drivers/media/i2c/s5k5baf.c

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