drivers/media/i2c/s5c73m3/s5c73m3-core.c

   1 /*
   2  * Samsung LSI S5C73M3 8M pixel camera driver
   3  *
   4  * Copyright (C) 2012, Samsung Electronics, Co., Ltd.
   5  * Sylwester Nawrocki <s.nawrocki@samsung.com>
   6  * Andrzej Hajda <a.hajda@samsung.com>
   7  *
   8  * This program is free software; you can redistribute it and/or
   9  * modify it under the terms of the GNU General Public License
  10  * version 2 as published by the Free Software Foundation.
  11  *
  12  * This program is distributed in the hope that it will be useful,
  13  * but WITHOUT ANY WARRANTY; without even the implied warranty of
  14  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
  15  * GNU General Public License for more details.
  16  */
  17
  18 #include <linux/sizes.h>
  19 #include <linux/delay.h>
  20 #include <linux/firmware.h>
  21 #include <linux/gpio.h>
  22 #include <linux/i2c.h>
  23 #include <linux/init.h>
  24 #include <linux/media.h>
  25 #include <linux/module.h>
  26 #include <linux/regulator/consumer.h>
  27 #include <linux/slab.h>
  28 #include <linux/spi/spi.h>
  29 #include <linux/videodev2.h>
  30 #include <media/media-entity.h>
  31 #include <media/v4l2-ctrls.h>
  32 #include <media/v4l2-device.h>
  33 #include <media/v4l2-subdev.h>
  34 #include <media/v4l2-mediabus.h>
  35 #include <media/s5c73m3.h>
  36
  37 #include "s5c73m3.h"
  38
  39 int s5c73m3_dbg;
  40 module_param_named(debug, s5c73m3_dbg, int, 0644);
  41
  42 static int boot_from_rom = 1;
  43 module_param(boot_from_rom, int, 0644);
  44
  45 static int update_fw;
  46 module_param(update_fw, int, 0644);
  47
  48 #define S5C73M3_EMBEDDED_DATA_MAXLEN    SZ_4K
  49
  50 static const char * const s5c73m3_supply_names[S5C73M3_MAX_SUPPLIES] = {
  51         "vdd-int",      /* Digital Core supply (1.2V), CAM_ISP_CORE_1.2V */
  52         "vdda",         /* Analog Core supply (1.2V), CAM_SENSOR_CORE_1.2V */
  53         "vdd-reg",      /* Regulator input supply (2.8V), CAM_SENSOR_A2.8V */
  54         "vddio-host",   /* Digital Host I/O power supply (1.8V...2.8V),
  55                            CAM_ISP_SENSOR_1.8V */
  56         "vddio-cis",    /* Digital CIS I/O power (1.2V...1.8V),
  57                            CAM_ISP_MIPI_1.2V */
  58         "vdd-af",       /* Lens, CAM_AF_2.8V */
  59 };
  60
  61 static const struct s5c73m3_frame_size s5c73m3_isp_resolutions[] = {
  62         { 320,  240,    COMM_CHG_MODE_YUV_320_240 },
  63         { 352,  288,    COMM_CHG_MODE_YUV_352_288 },
  64         { 640,  480,    COMM_CHG_MODE_YUV_640_480 },
  65         { 880,  720,    COMM_CHG_MODE_YUV_880_720 },
  66         { 960,  720,    COMM_CHG_MODE_YUV_960_720 },
  67         { 1008, 672,    COMM_CHG_MODE_YUV_1008_672 },
  68         { 1184, 666,    COMM_CHG_MODE_YUV_1184_666 },
  69         { 1280, 720,    COMM_CHG_MODE_YUV_1280_720 },
  70         { 1536, 864,    COMM_CHG_MODE_YUV_1536_864 },
  71         { 1600, 1200,   COMM_CHG_MODE_YUV_1600_1200 },
  72         { 1632, 1224,   COMM_CHG_MODE_YUV_1632_1224 },
  73         { 1920, 1080,   COMM_CHG_MODE_YUV_1920_1080 },
  74         { 1920, 1440,   COMM_CHG_MODE_YUV_1920_1440 },
  75         { 2304, 1296,   COMM_CHG_MODE_YUV_2304_1296 },
  76         { 3264, 2448,   COMM_CHG_MODE_YUV_3264_2448 },
  77 };
  78
  79 static const struct s5c73m3_frame_size s5c73m3_jpeg_resolutions[] = {
  80         { 640,  480,    COMM_CHG_MODE_JPEG_640_480 },
  81         { 800,  450,    COMM_CHG_MODE_JPEG_800_450 },
  82         { 800,  600,    COMM_CHG_MODE_JPEG_800_600 },
  83         { 1024, 768,    COMM_CHG_MODE_JPEG_1024_768 },
  84         { 1280, 720,    COMM_CHG_MODE_JPEG_1280_720 },
  85         { 1280, 960,    COMM_CHG_MODE_JPEG_1280_960 },
  86         { 1600, 900,    COMM_CHG_MODE_JPEG_1600_900 },
  87         { 1600, 1200,   COMM_CHG_MODE_JPEG_1600_1200 },
  88         { 2048, 1152,   COMM_CHG_MODE_JPEG_2048_1152 },
  89         { 2048, 1536,   COMM_CHG_MODE_JPEG_2048_1536 },
  90         { 2560, 1440,   COMM_CHG_MODE_JPEG_2560_1440 },
  91         { 2560, 1920,   COMM_CHG_MODE_JPEG_2560_1920 },
  92         { 3264, 1836,   COMM_CHG_MODE_JPEG_3264_1836 },
  93         { 3264, 2176,   COMM_CHG_MODE_JPEG_3264_2176 },
  94         { 3264, 2448,   COMM_CHG_MODE_JPEG_3264_2448 },
  95 };
  96
  97 static const struct s5c73m3_frame_size * const s5c73m3_resolutions[] = {
  98         [RES_ISP] = s5c73m3_isp_resolutions,
  99         [RES_JPEG] = s5c73m3_jpeg_resolutions
 100 };
 101
 102 static const int s5c73m3_resolutions_len[] = {
 103         [RES_ISP] = ARRAY_SIZE(s5c73m3_isp_resolutions),
 104         [RES_JPEG] = ARRAY_SIZE(s5c73m3_jpeg_resolutions)
 105 };
 106
 107 static const struct s5c73m3_interval s5c73m3_intervals[] = {
 108         { COMM_FRAME_RATE_FIXED_7FPS, {142857, 1000000}, {3264, 2448} },
 109         { COMM_FRAME_RATE_FIXED_15FPS, {66667, 1000000}, {3264, 2448} },
 110         { COMM_FRAME_RATE_FIXED_20FPS, {50000, 1000000}, {2304, 1296} },
 111         { COMM_FRAME_RATE_FIXED_30FPS, {33333, 1000000}, {2304, 1296} },
 112 };
 113
 114 #define S5C73M3_DEFAULT_FRAME_INTERVAL 3 /* 30 fps */
 115
 116 static void s5c73m3_fill_mbus_fmt(struct v4l2_mbus_framefmt *mf,
 117                                   const struct s5c73m3_frame_size *fs,
 118                                   u32 code)
 119 {
 120         mf->width = fs->width;
 121         mf->height = fs->height;
 122         mf->code = code;
 123         mf->colorspace = V4L2_COLORSPACE_JPEG;
 124         mf->field = V4L2_FIELD_NONE;
 125 }
 126
 127 static int s5c73m3_i2c_write(struct i2c_client *client, u16 addr, u16 data)
 128 {
 129         u8 buf[4] = { addr >> 8, addr & 0xff, data >> 8, data & 0xff };
 130
 131         int ret = i2c_master_send(client, buf, sizeof(buf));
 132
 133         v4l_dbg(4, s5c73m3_dbg, client, "%s: addr 0x%04x, data 0x%04x\n",
 134                  __func__, addr, data);
 135
 136         if (ret == 4)
 137                 return 0;
 138
 139         return ret < 0 ? ret : -EREMOTEIO;
 140 }
 141
 142 static int s5c73m3_i2c_read(struct i2c_client *client, u16 addr, u16 *data)
 143 {
 144         int ret;
 145         u8 rbuf[2], wbuf[2] = { addr >> 8, addr & 0xff };
 146         struct i2c_msg msg[2] = {
 147                 {
 148                         .addr = client->addr,
 149                         .flags = 0,
 150                         .len = sizeof(wbuf),
 151                         .buf = wbuf
 152                 }, {
 153                         .addr = client->addr,
 154                         .flags = I2C_M_RD,
 155                         .len = sizeof(rbuf),
 156                         .buf = rbuf
 157                 }
 158         };
 159         /*
 160          * Issue repeated START after writing 2 address bytes and
 161          * just one STOP only after reading the data bytes.
 162          */
 163         ret = i2c_transfer(client->adapter, msg, 2);
 164         if (ret == 2) {
 165                 *data = be16_to_cpup((u16 *)rbuf);
 166                 v4l2_dbg(4, s5c73m3_dbg, client,
 167                          "%s: addr: 0x%04x, data: 0x%04x\n",
 168                          __func__, addr, *data);
 169                 return 0;
 170         }
 171
 172         v4l2_err(client, "I2C read failed: addr: %04x, (%d)\n", addr, ret);
 173
 174         return ret >= 0 ? -EREMOTEIO : ret;
 175 }
 176
 177 int s5c73m3_write(struct s5c73m3 *state, u32 addr, u16 data)
 178 {
 179         struct i2c_client *client = state->i2c_client;
 180         int ret;
 181
 182         if ((addr ^ state->i2c_write_address) & 0xffff0000) {
 183                 ret = s5c73m3_i2c_write(client, REG_CMDWR_ADDRH, addr >> 16);
 184                 if (ret < 0) {
 185                         state->i2c_write_address = 0;
 186                         return ret;
 187                 }
 188         }
 189
 190         if ((addr ^ state->i2c_write_address) & 0xffff) {
 191                 ret = s5c73m3_i2c_write(client, REG_CMDWR_ADDRL, addr & 0xffff);
 192                 if (ret < 0) {
 193                         state->i2c_write_address = 0;
 194                         return ret;
 195                 }
 196         }
 197
 198         state->i2c_write_address = addr;
 199
 200         ret = s5c73m3_i2c_write(client, REG_CMDBUF_ADDR, data);
 201         if (ret < 0)
 202                 return ret;
 203
 204         state->i2c_write_address += 2;
 205
 206         return ret;
 207 }
 208
 209 int s5c73m3_read(struct s5c73m3 *state, u32 addr, u16 *data)
 210 {
 211         struct i2c_client *client = state->i2c_client;
 212         int ret;
 213
 214         if ((addr ^ state->i2c_read_address) & 0xffff0000) {
 215                 ret = s5c73m3_i2c_write(client, REG_CMDRD_ADDRH, addr >> 16);
 216                 if (ret < 0) {
 217                         state->i2c_read_address = 0;
 218                         return ret;
 219                 }
 220         }
 221
 222         if ((addr ^ state->i2c_read_address) & 0xffff) {
 223                 ret = s5c73m3_i2c_write(client, REG_CMDRD_ADDRL, addr & 0xffff);
 224                 if (ret < 0) {
 225                         state->i2c_read_address = 0;
 226                         return ret;
 227                 }
 228         }
 229
 230         state->i2c_read_address = addr;
 231
 232         ret = s5c73m3_i2c_read(client, REG_CMDBUF_ADDR, data);
 233         if (ret < 0)
 234                 return ret;
 235
 236         state->i2c_read_address += 2;
 237
 238         return ret;
 239 }
 240
 241 static int s5c73m3_check_status(struct s5c73m3 *state, unsigned int value)
 242 {
 243         unsigned long start = jiffies;
 244         unsigned long end = start + msecs_to_jiffies(2000);
 245         int ret = 0;
 246         u16 status;
 247         int count = 0;
 248
 249         while (time_is_after_jiffies(end)) {
 250                 ret = s5c73m3_read(state, REG_STATUS, &status);
 251                 if (ret < 0 || status == value)
 252                         break;
 253                 usleep_range(500, 1000);
 254                 ++count;
 255         }
 256
 257         if (count > 0)
 258                 v4l2_dbg(1, s5c73m3_dbg, &state->sensor_sd,
 259                          "status check took %dms\n",
 260                          jiffies_to_msecs(jiffies - start));
 261
 262         if (ret == 0 && status != value) {
 263                 u16 i2c_status = 0;
 264                 u16 i2c_seq_status = 0;
 265
 266                 s5c73m3_read(state, REG_I2C_STATUS, &i2c_status);
 267                 s5c73m3_read(state, REG_I2C_SEQ_STATUS, &i2c_seq_status);
 268
 269                 v4l2_err(&state->sensor_sd,
 270                          "wrong status %#x, expected: %#x, i2c_status: %#x/%#x\n",
 271                          status, value, i2c_status, i2c_seq_status);
 272
 273                 return -ETIMEDOUT;
 274         }
 275
 276         return ret;
 277 }
 278
 279 int s5c73m3_isp_command(struct s5c73m3 *state, u16 command, u16 data)
 280 {
 281         int ret;
 282
 283         ret = s5c73m3_check_status(state, REG_STATUS_ISP_COMMAND_COMPLETED);
 284         if (ret < 0)
 285                 return ret;
 286
 287         ret = s5c73m3_write(state, 0x00095000, command);
 288         if (ret < 0)
 289                 return ret;
 290
 291         ret = s5c73m3_write(state, 0x00095002, data);
 292         if (ret < 0)
 293                 return ret;
 294
 295         return s5c73m3_write(state, REG_STATUS, 0x0001);
 296 }
 297
 298 static int s5c73m3_isp_comm_result(struct s5c73m3 *state, u16 command,
 299                                    u16 *data)
 300 {
 301         return s5c73m3_read(state, COMM_RESULT_OFFSET + command, data);
 302 }
 303
 304 static int s5c73m3_set_af_softlanding(struct s5c73m3 *state)
 305 {
 306         unsigned long start = jiffies;
 307         u16 af_softlanding;
 308         int count = 0;
 309         int ret;
 310         const char *msg;
 311
 312         ret = s5c73m3_isp_command(state, COMM_AF_SOFTLANDING,
 313                                         COMM_AF_SOFTLANDING_ON);
 314         if (ret < 0) {
 315                 v4l2_info(&state->sensor_sd, "AF soft-landing failed\n");
 316                 return ret;
 317         }
 318
 319         for (;;) {
 320                 ret = s5c73m3_isp_comm_result(state, COMM_AF_SOFTLANDING,
 321                                                         &af_softlanding);
 322                 if (ret < 0) {
 323                         msg = "failed";
 324                         break;
 325                 }
 326                 if (af_softlanding == COMM_AF_SOFTLANDING_RES_COMPLETE) {
 327                         msg = "succeeded";
 328                         break;
 329                 }
 330                 if (++count > 100) {
 331                         ret = -ETIME;
 332                         msg = "timed out";
 333                         break;
 334                 }
 335                 msleep(25);
 336         }
 337
 338         v4l2_info(&state->sensor_sd, "AF soft-landing %s after %dms\n",
 339                   msg, jiffies_to_msecs(jiffies - start));
 340
 341         return ret;
 342 }
 343
 344 static int s5c73m3_load_fw(struct v4l2_subdev *sd)
 345 {
 346         struct s5c73m3 *state = sensor_sd_to_s5c73m3(sd);
 347         struct i2c_client *client = state->i2c_client;
 348         const struct firmware *fw;
 349         int ret;
 350         char fw_name[20];
 351
 352         snprintf(fw_name, sizeof(fw_name), "SlimISP_%.2s.bin",
 353                                                         state->fw_file_version);
 354         ret = request_firmware(&fw, fw_name, &client->dev);
 355         if (ret < 0) {
 356                 v4l2_err(sd, "Firmware request failed (%s)\n", fw_name);
 357                 return -EINVAL;
 358         }
 359
 360         v4l2_info(sd, "Loading firmware (%s, %zu B)\n", fw_name, fw->size);
 361
 362         ret = s5c73m3_spi_write(state, fw->data, fw->size, 64);
 363
 364         if (ret >= 0)
 365                 state->isp_ready = 1;
 366         else
 367                 v4l2_err(sd, "SPI write failed\n");
 368
 369         release_firmware(fw);
 370
 371         return ret;
 372 }
 373
 374 static int s5c73m3_set_frame_size(struct s5c73m3 *state)
 375 {
 376         const struct s5c73m3_frame_size *prev_size =
 377                                         state->sensor_pix_size[RES_ISP];
 378         const struct s5c73m3_frame_size *cap_size =
 379                                         state->sensor_pix_size[RES_JPEG];
 380         unsigned int chg_mode;
 381
 382         v4l2_dbg(1, s5c73m3_dbg, &state->sensor_sd,
 383                  "Preview size: %dx%d, reg_val: 0x%x\n",
 384                  prev_size->width, prev_size->height, prev_size->reg_val);
 385
 386         chg_mode = prev_size->reg_val | COMM_CHG_MODE_NEW;
 387
 388         if (state->mbus_code == S5C73M3_JPEG_FMT) {
 389                 v4l2_dbg(1, s5c73m3_dbg, &state->sensor_sd,
 390                          "Capture size: %dx%d, reg_val: 0x%x\n",
 391                          cap_size->width, cap_size->height, cap_size->reg_val);
 392                 chg_mode |= cap_size->reg_val;
 393         }
 394
 395         return s5c73m3_isp_command(state, COMM_CHG_MODE, chg_mode);
 396 }
 397
 398 static int s5c73m3_set_frame_rate(struct s5c73m3 *state)
 399 {
 400         int ret;
 401
 402         if (state->ctrls.stabilization->val)
 403                 return 0;
 404
 405         if (WARN_ON(state->fiv == NULL))
 406                 return -EINVAL;
 407
 408         ret = s5c73m3_isp_command(state, COMM_FRAME_RATE, state->fiv->fps_reg);
 409         if (!ret)
 410                 state->apply_fiv = 0;
 411
 412         return ret;
 413 }
 414
 415 static int __s5c73m3_s_stream(struct s5c73m3 *state, struct v4l2_subdev *sd,
 416                                                                 int on)
 417 {
 418         u16 mode;
 419         int ret;
 420
 421         if (on && state->apply_fmt) {
 422                 if (state->mbus_code == S5C73M3_JPEG_FMT)
 423                         mode = COMM_IMG_OUTPUT_INTERLEAVED;
 424                 else
 425                         mode = COMM_IMG_OUTPUT_YUV;
 426
 427                 ret = s5c73m3_isp_command(state, COMM_IMG_OUTPUT, mode);
 428                 if (!ret)
 429                         ret = s5c73m3_set_frame_size(state);
 430                 if (ret)
 431                         return ret;
 432                 state->apply_fmt = 0;
 433         }
 434
 435         ret = s5c73m3_isp_command(state, COMM_SENSOR_STREAMING, !!on);
 436         if (ret)
 437                 return ret;
 438
 439         state->streaming = !!on;
 440
 441         if (!on)
 442                 return ret;
 443
 444         if (state->apply_fiv) {
 445                 ret = s5c73m3_set_frame_rate(state);
 446                 if (ret < 0)
 447                         v4l2_err(sd, "Error setting frame rate(%d)\n", ret);
 448         }
 449
 450         return s5c73m3_check_status(state, REG_STATUS_ISP_COMMAND_COMPLETED);
 451 }
 452
 453 static int s5c73m3_oif_s_stream(struct v4l2_subdev *sd, int on)
 454 {
 455         struct s5c73m3 *state = oif_sd_to_s5c73m3(sd);
 456         int ret;
 457
 458         mutex_lock(&state->lock);
 459         ret = __s5c73m3_s_stream(state, sd, on);
 460         mutex_unlock(&state->lock);
 461
 462         return ret;
 463 }
 464
 465 static int s5c73m3_system_status_wait(struct s5c73m3 *state, u32 value,
 466                                       unsigned int delay, unsigned int steps)
 467 {
 468         u16 reg = 0;
 469
 470         while (steps-- > 0) {
 471                 int ret = s5c73m3_read(state, 0x30100010, &reg);
 472                 if (ret < 0)
 473                         return ret;
 474                 if (reg == value)
 475                         return 0;
 476                 usleep_range(delay, delay + 25);
 477         }
 478         return -ETIMEDOUT;
 479 }
 480
 481 static int s5c73m3_read_fw_version(struct s5c73m3 *state)
 482 {
 483         struct v4l2_subdev *sd = &state->sensor_sd;
 484         int i, ret;
 485         u16 data[2];
 486         int offset;
 487
 488         offset = state->isp_ready ? 0x60 : 0;
 489
 490         for (i = 0; i < S5C73M3_SENSOR_FW_LEN / 2; i++) {
 491                 ret = s5c73m3_read(state, offset + i * 2, data);
 492                 if (ret < 0)
 493                         return ret;
 494                 state->sensor_fw[i * 2] = (char)(*data & 0xff);
 495                 state->sensor_fw[i * 2 + 1] = (char)(*data >> 8);
 496         }
 497         state->sensor_fw[S5C73M3_SENSOR_FW_LEN] = '\0';
 498
 499
 500         for (i = 0; i < S5C73M3_SENSOR_TYPE_LEN / 2; i++) {
 501                 ret = s5c73m3_read(state, offset + 6 + i * 2, data);
 502                 if (ret < 0)
 503                         return ret;
 504                 state->sensor_type[i * 2] = (char)(*data & 0xff);
 505                 state->sensor_type[i * 2 + 1] = (char)(*data >> 8);
 506         }
 507         state->sensor_type[S5C73M3_SENSOR_TYPE_LEN] = '\0';
 508
 509         ret = s5c73m3_read(state, offset + 0x14, data);
 510         if (ret >= 0) {
 511                 ret = s5c73m3_read(state, offset + 0x16, data + 1);
 512                 if (ret >= 0)
 513                         state->fw_size = data[0] + (data[1] << 16);
 514         }
 515
 516         v4l2_info(sd, "Sensor type: %s, FW version: %s\n",
 517                   state->sensor_type, state->sensor_fw);
 518         return ret;
 519 }
 520
 521 static int s5c73m3_fw_update_from(struct s5c73m3 *state)
 522 {
 523         struct v4l2_subdev *sd = &state->sensor_sd;
 524         u16 status = COMM_FW_UPDATE_NOT_READY;
 525         int ret;
 526         int count = 0;
 527
 528         v4l2_warn(sd, "Updating F-ROM firmware.\n");
 529         do {
 530                 if (status == COMM_FW_UPDATE_NOT_READY) {
 531                         ret = s5c73m3_isp_command(state, COMM_FW_UPDATE, 0);
 532                         if (ret < 0)
 533                                 return ret;
 534                 }
 535
 536                 ret = s5c73m3_read(state, 0x00095906, &status);
 537                 if (ret < 0)
 538                         return ret;
 539                 switch (status) {
 540                 case COMM_FW_UPDATE_FAIL:
 541                         v4l2_warn(sd, "Updating F-ROM firmware failed.\n");
 542                         return -EIO;
 543                 case COMM_FW_UPDATE_SUCCESS:
 544                         v4l2_warn(sd, "Updating F-ROM firmware finished.\n");
 545                         return 0;
 546                 }
 547                 ++count;
 548                 msleep(20);
 549         } while (count < 500);
 550
 551         v4l2_warn(sd, "Updating F-ROM firmware timed-out.\n");
 552         return -ETIMEDOUT;
 553 }
 554
 555 static int s5c73m3_spi_boot(struct s5c73m3 *state, bool load_fw)
 556 {
 557         struct v4l2_subdev *sd = &state->sensor_sd;
 558         int ret;
 559
 560         /* Run ARM MCU */
 561         ret = s5c73m3_write(state, 0x30000004, 0xffff);
 562         if (ret < 0)
 563                 return ret;
 564
 565         usleep_range(400, 500);
 566
 567         /* Check booting status */
 568         ret = s5c73m3_system_status_wait(state, 0x0c, 100, 3);
 569         if (ret < 0) {
 570                 v4l2_err(sd, "booting failed: %d\n", ret);
 571                 return ret;
 572         }
 573
 574         /* P,M,S and Boot Mode */
 575         ret = s5c73m3_write(state, 0x30100014, 0x2146);
 576         if (ret < 0)
 577                 return ret;
 578
 579         ret = s5c73m3_write(state, 0x30100010, 0x210c);
 580         if (ret < 0)
 581                 return ret;
 582
 583         usleep_range(200, 250);
 584
 585         /* Check SPI status */
 586         ret = s5c73m3_system_status_wait(state, 0x210d, 100, 300);
 587         if (ret < 0)
 588                 v4l2_err(sd, "SPI not ready: %d\n", ret);
 589
 590         /* Firmware download over SPI */
 591         if (load_fw)
 592                 s5c73m3_load_fw(sd);
 593
 594         /* MCU reset */
 595         ret = s5c73m3_write(state, 0x30000004, 0xfffd);
 596         if (ret < 0)
 597                 return ret;
 598
 599         /* Remap */
 600         ret = s5c73m3_write(state, 0x301000a4, 0x0183);
 601         if (ret < 0)
 602                 return ret;
 603
 604         /* MCU restart */
 605         ret = s5c73m3_write(state, 0x30000004, 0xffff);
 606         if (ret < 0 || !load_fw)
 607                 return ret;
 608
 609         ret = s5c73m3_read_fw_version(state);
 610         if (ret < 0)
 611                 return ret;
 612
 613         if (load_fw && update_fw) {
 614                 ret = s5c73m3_fw_update_from(state);
 615                 update_fw = 0;
 616         }
 617
 618         return ret;
 619 }
 620
 621 static int s5c73m3_set_timing_register_for_vdd(struct s5c73m3 *state)
 622 {
 623         static const u32 regs[][2] = {
 624                 { 0x30100018, 0x0618 },
 625                 { 0x3010001c, 0x10c1 },
 626                 { 0x30100020, 0x249e }
 627         };
 628         int ret;
 629         int i;
 630
 631         for (i = 0; i < ARRAY_SIZE(regs); i++) {
 632                 ret = s5c73m3_write(state, regs[i][0], regs[i][1]);
 633                 if (ret < 0)
 634                         return ret;
 635         }
 636
 637         return 0;
 638 }
 639
 640 static void s5c73m3_set_fw_file_version(struct s5c73m3 *state)
 641 {
 642         switch (state->sensor_fw[0]) {
 643         case 'G':
 644         case 'O':
 645                 state->fw_file_version[0] = 'G';
 646                 break;
 647         case 'S':
 648         case 'Z':
 649                 state->fw_file_version[0] = 'Z';
 650                 break;
 651         }
 652
 653         switch (state->sensor_fw[1]) {
 654         case 'C'...'F':
 655                 state->fw_file_version[1] = state->sensor_fw[1];
 656                 break;
 657         }
 658 }
 659
 660 static int s5c73m3_get_fw_version(struct s5c73m3 *state)
 661 {
 662         struct v4l2_subdev *sd = &state->sensor_sd;
 663         int ret;
 664
 665         /* Run ARM MCU */
 666         ret = s5c73m3_write(state, 0x30000004, 0xffff);
 667         if (ret < 0)
 668                 return ret;
 669         usleep_range(400, 500);
 670
 671         /* Check booting status */
 672         ret = s5c73m3_system_status_wait(state, 0x0c, 100, 3);
 673         if (ret < 0) {
 674
 675                 v4l2_err(sd, "%s: booting failed: %d\n", __func__, ret);
 676                 return ret;
 677         }
 678
 679         /* Change I/O Driver Current in order to read from F-ROM */
 680         ret = s5c73m3_write(state, 0x30100120, 0x0820);
 681         ret = s5c73m3_write(state, 0x30100124, 0x0820);
 682
 683         /* Offset Setting */
 684         ret = s5c73m3_write(state, 0x00010418, 0x0008);
 685
 686         /* P,M,S and Boot Mode */
 687         ret = s5c73m3_write(state, 0x30100014, 0x2146);
 688         if (ret < 0)
 689                 return ret;
 690         ret = s5c73m3_write(state, 0x30100010, 0x230c);
 691         if (ret < 0)
 692                 return ret;
 693
 694         usleep_range(200, 250);
 695
 696         /* Check SPI status */
 697         ret = s5c73m3_system_status_wait(state, 0x230e, 100, 300);
 698         if (ret < 0)
 699                 v4l2_err(sd, "SPI not ready: %d\n", ret);
 700
 701         /* ARM reset */
 702         ret = s5c73m3_write(state, 0x30000004, 0xfffd);
 703         if (ret < 0)
 704                 return ret;
 705
 706         /* Remap */
 707         ret = s5c73m3_write(state, 0x301000a4, 0x0183);
 708         if (ret < 0)
 709                 return ret;
 710
 711         s5c73m3_set_timing_register_for_vdd(state);
 712
 713         ret = s5c73m3_read_fw_version(state);
 714
 715         s5c73m3_set_fw_file_version(state);
 716
 717         return ret;
 718 }
 719
 720 static int s5c73m3_rom_boot(struct s5c73m3 *state, bool load_fw)
 721 {
 722         static const u32 boot_regs[][2] = {
 723                 { 0x3100010c, 0x0044 },
 724                 { 0x31000108, 0x000d },
 725                 { 0x31000304, 0x0001 },
 726                 { 0x00010000, 0x5800 },
 727                 { 0x00010002, 0x0002 },
 728                 { 0x31000000, 0x0001 },
 729                 { 0x30100014, 0x1b85 },
 730                 { 0x30100010, 0x230c }
 731         };
 732         struct v4l2_subdev *sd = &state->sensor_sd;
 733         int i, ret;
 734
 735         /* Run ARM MCU */
 736         ret = s5c73m3_write(state, 0x30000004, 0xffff);
 737         if (ret < 0)
 738                 return ret;
 739         usleep_range(400, 450);
 740
 741         /* Check booting status */
 742         ret = s5c73m3_system_status_wait(state, 0x0c, 100, 4);
 743         if (ret < 0) {
 744                 v4l2_err(sd, "Booting failed: %d\n", ret);
 745                 return ret;
 746         }
 747
 748         for (i = 0; i < ARRAY_SIZE(boot_regs); i++) {
 749                 ret = s5c73m3_write(state, boot_regs[i][0], boot_regs[i][1]);
 750                 if (ret < 0)
 751                         return ret;
 752         }
 753         msleep(200);
 754
 755         /* Check the binary read status */
 756         ret = s5c73m3_system_status_wait(state, 0x230e, 1000, 150);
 757         if (ret < 0) {
 758                 v4l2_err(sd, "Binary read failed: %d\n", ret);
 759                 return ret;
 760         }
 761
 762         /* ARM reset */
 763         ret = s5c73m3_write(state, 0x30000004, 0xfffd);
 764         if (ret < 0)
 765                 return ret;
 766         /* Remap */
 767         ret = s5c73m3_write(state, 0x301000a4, 0x0183);
 768         if (ret < 0)
 769                 return ret;
 770         /* MCU re-start */
 771         ret = s5c73m3_write(state, 0x30000004, 0xffff);
 772         if (ret < 0)
 773                 return ret;
 774
 775         state->isp_ready = 1;
 776
 777         return s5c73m3_read_fw_version(state);
 778 }
 779
 780 static int s5c73m3_isp_init(struct s5c73m3 *state)
 781 {
 782         int ret;
 783
 784         state->i2c_read_address = 0;
 785         state->i2c_write_address = 0;
 786
 787         ret = s5c73m3_i2c_write(state->i2c_client, AHB_MSB_ADDR_PTR, 0x3310);
 788         if (ret < 0)
 789                 return ret;
 790
 791         if (boot_from_rom)
 792                 return s5c73m3_rom_boot(state, true);
 793         else
 794                 return s5c73m3_spi_boot(state, true);
 795 }
 796
 797 static const struct s5c73m3_frame_size *s5c73m3_find_frame_size(
 798                                         struct v4l2_mbus_framefmt *fmt,
 799                                         enum s5c73m3_resolution_types idx)
 800 {
 801         const struct s5c73m3_frame_size *fs;
 802         const struct s5c73m3_frame_size *best_fs;
 803         int best_dist = INT_MAX;
 804         int i;
 805
 806         fs = s5c73m3_resolutions[idx];
 807         best_fs = NULL;
 808         for (i = 0; i < s5c73m3_resolutions_len[idx]; ++i) {
 809                 int dist = abs(fs->width - fmt->width) +
 810                                                 abs(fs->height - fmt->height);
 811                 if (dist < best_dist) {
 812                         best_dist = dist;
 813                         best_fs = fs;
 814                 }
 815                 ++fs;
 816         }
 817
 818         return best_fs;
 819 }
 820
 821 static void s5c73m3_oif_try_format(struct s5c73m3 *state,
 822                                    struct v4l2_subdev_fh *fh,
 823                                    struct v4l2_subdev_format *fmt,
 824                                    const struct s5c73m3_frame_size **fs)
 825 {
 826         u32 code;
 827
 828         switch (fmt->pad) {
 829         case OIF_ISP_PAD:
 830                 *fs = s5c73m3_find_frame_size(&fmt->format, RES_ISP);
 831                 code = S5C73M3_ISP_FMT;
 832                 break;
 833         case OIF_JPEG_PAD:
 834                 *fs = s5c73m3_find_frame_size(&fmt->format, RES_JPEG);
 835                 code = S5C73M3_JPEG_FMT;
 836                 break;
 837         case OIF_SOURCE_PAD:
 838         default:
 839                 if (fmt->format.code == S5C73M3_JPEG_FMT)
 840                         code = S5C73M3_JPEG_FMT;
 841                 else
 842                         code = S5C73M3_ISP_FMT;
 843
 844                 if (fmt->which == V4L2_SUBDEV_FORMAT_ACTIVE)
 845                         *fs = state->oif_pix_size[RES_ISP];
 846                 else
 847                         *fs = s5c73m3_find_frame_size(
 848                                                 v4l2_subdev_get_try_format(fh,
 849                                                         OIF_ISP_PAD),
 850                                                 RES_ISP);
 851                 break;
 852         }
 853
 854         s5c73m3_fill_mbus_fmt(&fmt->format, *fs, code);
 855 }
 856
 857 static void s5c73m3_try_format(struct s5c73m3 *state,
 858                               struct v4l2_subdev_fh *fh,
 859                               struct v4l2_subdev_format *fmt,
 860                               const struct s5c73m3_frame_size **fs)
 861 {
 862         u32 code;
 863
 864         if (fmt->pad == S5C73M3_ISP_PAD) {
 865                 *fs = s5c73m3_find_frame_size(&fmt->format, RES_ISP);
 866                 code = S5C73M3_ISP_FMT;
 867         } else {
 868                 *fs = s5c73m3_find_frame_size(&fmt->format, RES_JPEG);
 869                 code = S5C73M3_JPEG_FMT;
 870         }
 871
 872         s5c73m3_fill_mbus_fmt(&fmt->format, *fs, code);
 873 }
 874
 875 static int s5c73m3_oif_g_frame_interval(struct v4l2_subdev *sd,
 876                                    struct v4l2_subdev_frame_interval *fi)
 877 {
 878         struct s5c73m3 *state = oif_sd_to_s5c73m3(sd);
 879
 880         if (fi->pad != OIF_SOURCE_PAD)
 881                 return -EINVAL;
 882
 883         mutex_lock(&state->lock);
 884         fi->interval = state->fiv->interval;
 885         mutex_unlock(&state->lock);
 886
 887         return 0;
 888 }
 889
 890 static int __s5c73m3_set_frame_interval(struct s5c73m3 *state,
 891                                         struct v4l2_subdev_frame_interval *fi)
 892 {
 893         const struct s5c73m3_frame_size *prev_size =
 894                                                 state->sensor_pix_size[RES_ISP];
 895         const struct s5c73m3_interval *fiv = &s5c73m3_intervals[0];
 896         unsigned int ret, min_err = UINT_MAX;
 897         unsigned int i, fr_time;
 898
 899         if (fi->interval.denominator == 0)
 900                 return -EINVAL;
 901
 902         fr_time = fi->interval.numerator * 1000 / fi->interval.denominator;
 903
 904         for (i = 0; i < ARRAY_SIZE(s5c73m3_intervals); i++) {
 905                 const struct s5c73m3_interval *iv = &s5c73m3_intervals[i];
 906
 907                 if (prev_size->width > iv->size.width ||
 908                     prev_size->height > iv->size.height)
 909                         continue;
 910
 911                 ret = abs(iv->interval.numerator / 1000 - fr_time);
 912                 if (ret < min_err) {
 913                         fiv = iv;
 914                         min_err = ret;
 915                 }
 916         }
 917         state->fiv = fiv;
 918
 919         v4l2_dbg(1, s5c73m3_dbg, &state->sensor_sd,
 920                  "Changed frame interval to %u us\n", fiv->interval.numerator);
 921         return 0;
 922 }
 923
 924 static int s5c73m3_oif_s_frame_interval(struct v4l2_subdev *sd,
 925                                    struct v4l2_subdev_frame_interval *fi)
 926 {
 927         struct s5c73m3 *state = oif_sd_to_s5c73m3(sd);
 928         int ret;
 929
 930         if (fi->pad != OIF_SOURCE_PAD)
 931                 return -EINVAL;
 932
 933         v4l2_dbg(1, s5c73m3_dbg, sd, "Setting %d/%d frame interval\n",
 934                  fi->interval.numerator, fi->interval.denominator);
 935
 936         mutex_lock(&state->lock);
 937
 938         ret = __s5c73m3_set_frame_interval(state, fi);
 939         if (!ret) {
 940                 if (state->streaming)
 941                         ret = s5c73m3_set_frame_rate(state);
 942                 else
 943                         state->apply_fiv = 1;
 944         }
 945         mutex_unlock(&state->lock);
 946         return ret;
 947 }
 948
 949 static int s5c73m3_oif_enum_frame_interval(struct v4l2_subdev *sd,
 950                               struct v4l2_subdev_fh *fh,
 951                               struct v4l2_subdev_frame_interval_enum *fie)
 952 {
 953         struct s5c73m3 *state = oif_sd_to_s5c73m3(sd);
 954         const struct s5c73m3_interval *fi;
 955         int ret = 0;
 956
 957         if (fie->pad != OIF_SOURCE_PAD)
 958                 return -EINVAL;
 959         if (fie->index >= ARRAY_SIZE(s5c73m3_intervals))
 960                 return -EINVAL;
 961
 962         mutex_lock(&state->lock);
 963         fi = &s5c73m3_intervals[fie->index];
 964         if (fie->width > fi->size.width || fie->height > fi->size.height)
 965                 ret = -EINVAL;
 966         else
 967                 fie->interval = fi->interval;
 968         mutex_unlock(&state->lock);
 969
 970         return ret;
 971 }
 972
 973 static int s5c73m3_oif_get_pad_code(int pad, int index)
 974 {
 975         if (pad == OIF_SOURCE_PAD) {
 976                 if (index > 1)
 977                         return -EINVAL;
 978                 return (index == 0) ? S5C73M3_ISP_FMT : S5C73M3_JPEG_FMT;
 979         }
 980
 981         if (index > 0)
 982                 return -EINVAL;
 983
 984         return (pad == OIF_ISP_PAD) ? S5C73M3_ISP_FMT : S5C73M3_JPEG_FMT;
 985 }
 986
 987 static int s5c73m3_get_fmt(struct v4l2_subdev *sd,
 988                            struct v4l2_subdev_fh *fh,
 989                            struct v4l2_subdev_format *fmt)
 990 {
 991         struct s5c73m3 *state = sensor_sd_to_s5c73m3(sd);
 992         const struct s5c73m3_frame_size *fs;
 993         u32 code;
 994
 995         if (fmt->which == V4L2_SUBDEV_FORMAT_TRY) {
 996                 fmt->format = *v4l2_subdev_get_try_format(fh, fmt->pad);
 997                 return 0;
 998         }
 999
1000         mutex_lock(&state->lock);
1001
1002         switch (fmt->pad) {
1003         case S5C73M3_ISP_PAD:
1004                 code = S5C73M3_ISP_FMT;
1005                 fs = state->sensor_pix_size[RES_ISP];
1006                 break;
1007         case S5C73M3_JPEG_PAD:
1008                 code = S5C73M3_JPEG_FMT;
1009                 fs = state->sensor_pix_size[RES_JPEG];
1010                 break;
1011         default:
1012                 mutex_unlock(&state->lock);
1013                 return -EINVAL;
1014         }
1015         s5c73m3_fill_mbus_fmt(&fmt->format, fs, code);
1016
1017         mutex_unlock(&state->lock);
1018         return 0;
1019 }
1020
1021 static int s5c73m3_oif_get_fmt(struct v4l2_subdev *sd,
1022                            struct v4l2_subdev_fh *fh,
1023                            struct v4l2_subdev_format *fmt)
1024 {
1025         struct s5c73m3 *state = oif_sd_to_s5c73m3(sd);
1026         const struct s5c73m3_frame_size *fs;
1027         u32 code;
1028
1029         if (fmt->which == V4L2_SUBDEV_FORMAT_TRY) {
1030                 fmt->format = *v4l2_subdev_get_try_format(fh, fmt->pad);
1031                 return 0;
1032         }
1033
1034         mutex_lock(&state->lock);
1035
1036         switch (fmt->pad) {
1037         case OIF_ISP_PAD:
1038                 code = S5C73M3_ISP_FMT;
1039                 fs = state->oif_pix_size[RES_ISP];
1040                 break;
1041         case OIF_JPEG_PAD:
1042                 code = S5C73M3_JPEG_FMT;
1043                 fs = state->oif_pix_size[RES_JPEG];
1044                 break;
1045         case OIF_SOURCE_PAD:
1046                 code = state->mbus_code;
1047                 fs = state->oif_pix_size[RES_ISP];
1048                 break;
1049         default:
1050                 mutex_unlock(&state->lock);
1051                 return -EINVAL;
1052         }
1053         s5c73m3_fill_mbus_fmt(&fmt->format, fs, code);
1054
1055         mutex_unlock(&state->lock);
1056         return 0;
1057 }
1058
1059 static int s5c73m3_set_fmt(struct v4l2_subdev *sd,
1060                            struct v4l2_subdev_fh *fh,
1061                            struct v4l2_subdev_format *fmt)
1062 {
1063         const struct s5c73m3_frame_size *frame_size = NULL;
1064         struct s5c73m3 *state = sensor_sd_to_s5c73m3(sd);
1065         struct v4l2_mbus_framefmt *mf;
1066         int ret = 0;
1067
1068         mutex_lock(&state->lock);
1069
1070         s5c73m3_try_format(state, fh, fmt, &frame_size);
1071
1072         if (fmt->which == V4L2_SUBDEV_FORMAT_TRY) {
1073                 mf = v4l2_subdev_get_try_format(fh, fmt->pad);
1074                 *mf = fmt->format;
1075         } else {
1076                 switch (fmt->pad) {
1077                 case S5C73M3_ISP_PAD:
1078                         state->sensor_pix_size[RES_ISP] = frame_size;
1079                         break;
1080                 case S5C73M3_JPEG_PAD:
1081                         state->sensor_pix_size[RES_JPEG] = frame_size;
1082                         break;
1083                 default:
1084                         ret = -EBUSY;
1085                 }
1086
1087                 if (state->streaming)
1088                         ret = -EBUSY;
1089                 else
1090                         state->apply_fmt = 1;
1091         }
1092
1093         mutex_unlock(&state->lock);
1094
1095         return ret;
1096 }
1097
1098 static int s5c73m3_oif_set_fmt(struct v4l2_subdev *sd,
1099                          struct v4l2_subdev_fh *fh,
1100                          struct v4l2_subdev_format *fmt)
1101 {
1102         const struct s5c73m3_frame_size *frame_size = NULL;
1103         struct s5c73m3 *state = oif_sd_to_s5c73m3(sd);
1104         struct v4l2_mbus_framefmt *mf;
1105         int ret = 0;
1106
1107         mutex_lock(&state->lock);
1108
1109         s5c73m3_oif_try_format(state, fh, fmt, &frame_size);
1110
1111         if (fmt->which == V4L2_SUBDEV_FORMAT_TRY) {
1112                 mf = v4l2_subdev_get_try_format(fh, fmt->pad);
1113                 *mf = fmt->format;
1114                 if (fmt->pad == OIF_ISP_PAD) {
1115                         mf = v4l2_subdev_get_try_format(fh, OIF_SOURCE_PAD);
1116                         mf->width = fmt->format.width;
1117                         mf->height = fmt->format.height;
1118                 }
1119         } else {
1120                 switch (fmt->pad) {
1121                 case OIF_ISP_PAD:
1122                         state->oif_pix_size[RES_ISP] = frame_size;
1123                         break;
1124                 case OIF_JPEG_PAD:
1125                         state->oif_pix_size[RES_JPEG] = frame_size;
1126                         break;
1127                 case OIF_SOURCE_PAD:
1128                         state->mbus_code = fmt->format.code;
1129                         break;
1130                 default:
1131                         ret = -EBUSY;
1132                 }
1133
1134                 if (state->streaming)
1135                         ret = -EBUSY;
1136                 else
1137                         state->apply_fmt = 1;
1138         }
1139
1140         mutex_unlock(&state->lock);
1141
1142         return ret;
1143 }
1144
1145 static int s5c73m3_oif_get_frame_desc(struct v4l2_subdev *sd, unsigned int pad,
1146                                   struct v4l2_mbus_frame_desc *fd)
1147 {
1148         struct s5c73m3 *state = oif_sd_to_s5c73m3(sd);
1149         int i;
1150
1151         if (pad != OIF_SOURCE_PAD || fd == NULL)
1152                 return -EINVAL;
1153
1154         mutex_lock(&state->lock);
1155         fd->num_entries = 2;
1156         for (i = 0; i < fd->num_entries; i++)
1157                 fd->entry[i] = state->frame_desc.entry[i];
1158         mutex_unlock(&state->lock);
1159
1160         return 0;
1161 }
1162
1163 static int s5c73m3_oif_set_frame_desc(struct v4l2_subdev *sd, unsigned int pad,
1164                                       struct v4l2_mbus_frame_desc *fd)
1165 {
1166         struct s5c73m3 *state = oif_sd_to_s5c73m3(sd);
1167         struct v4l2_mbus_frame_desc *frame_desc = &state->frame_desc;
1168         int i;
1169
1170         if (pad != OIF_SOURCE_PAD || fd == NULL)
1171                 return -EINVAL;
1172
1173         fd->entry[0].length = 10 * SZ_1M;
1174         fd->entry[1].length = max_t(u32, fd->entry[1].length,
1175                                     S5C73M3_EMBEDDED_DATA_MAXLEN);
1176         fd->num_entries = 2;
1177
1178         mutex_lock(&state->lock);
1179         for (i = 0; i < fd->num_entries; i++)
1180                 frame_desc->entry[i] = fd->entry[i];
1181         mutex_unlock(&state->lock);
1182
1183         return 0;
1184 }
1185
1186 static int s5c73m3_enum_mbus_code(struct v4l2_subdev *sd,
1187                                   struct v4l2_subdev_fh *fh,
1188                                   struct v4l2_subdev_mbus_code_enum *code)
1189 {
1190         static const int codes[] = {
1191                         [S5C73M3_ISP_PAD] = S5C73M3_ISP_FMT,
1192                         [S5C73M3_JPEG_PAD] = S5C73M3_JPEG_FMT};
1193
1194         if (code->index > 0 || code->pad >= S5C73M3_NUM_PADS)
1195                 return -EINVAL;
1196
1197         code->code = codes[code->pad];
1198
1199         return 0;
1200 }
1201
1202 static int s5c73m3_oif_enum_mbus_code(struct v4l2_subdev *sd,
1203                                 struct v4l2_subdev_fh *fh,
1204                                 struct v4l2_subdev_mbus_code_enum *code)
1205 {
1206         int ret;
1207
1208         ret = s5c73m3_oif_get_pad_code(code->pad, code->index);
1209         if (ret < 0)
1210                 return ret;
1211
1212         code->code = ret;
1213
1214         return 0;
1215 }
1216
1217 static int s5c73m3_enum_frame_size(struct v4l2_subdev *sd,
1218                                    struct v4l2_subdev_fh *fh,
1219                                    struct v4l2_subdev_frame_size_enum *fse)
1220 {
1221         int idx;
1222
1223         if (fse->pad == S5C73M3_ISP_PAD) {
1224                 if (fse->code != S5C73M3_ISP_FMT)
1225                         return -EINVAL;
1226                 idx = RES_ISP;
1227         } else{
1228                 if (fse->code != S5C73M3_JPEG_FMT)
1229                         return -EINVAL;
1230                 idx = RES_JPEG;
1231         }
1232
1233         if (fse->index >= s5c73m3_resolutions_len[idx])
1234                 return -EINVAL;
1235
1236         fse->min_width  = s5c73m3_resolutions[idx][fse->index].width;
1237         fse->max_width  = fse->min_width;
1238         fse->max_height = s5c73m3_resolutions[idx][fse->index].height;
1239         fse->min_height = fse->max_height;
1240
1241         return 0;
1242 }
1243
1244 static int s5c73m3_oif_enum_frame_size(struct v4l2_subdev *sd,
1245                                    struct v4l2_subdev_fh *fh,
1246                                    struct v4l2_subdev_frame_size_enum *fse)
1247 {
1248         int idx;
1249
1250         if (fse->pad == OIF_SOURCE_PAD) {
1251                 if (fse->index > 0)
1252                         return -EINVAL;
1253
1254                 switch (fse->code) {
1255                 case S5C73M3_JPEG_FMT:
1256                 case S5C73M3_ISP_FMT: {
1257                         struct v4l2_mbus_framefmt *mf =
1258                                 v4l2_subdev_get_try_format(fh, OIF_ISP_PAD);
1259
1260                         fse->max_width = fse->min_width = mf->width;
1261                         fse->max_height = fse->min_height = mf->height;
1262                         return 0;
1263                 }
1264                 default:
1265                         return -EINVAL;
1266                 }
1267         }
1268
1269         if (fse->code != s5c73m3_oif_get_pad_code(fse->pad, 0))
1270                 return -EINVAL;
1271
1272         if (fse->pad == OIF_JPEG_PAD)
1273                 idx = RES_JPEG;
1274         else
1275                 idx = RES_ISP;
1276
1277         if (fse->index >= s5c73m3_resolutions_len[idx])
1278                 return -EINVAL;
1279
1280         fse->min_width  = s5c73m3_resolutions[idx][fse->index].width;
1281         fse->max_width  = fse->min_width;
1282         fse->max_height = s5c73m3_resolutions[idx][fse->index].height;
1283         fse->min_height = fse->max_height;
1284
1285         return 0;
1286 }
1287
1288 static int s5c73m3_oif_log_status(struct v4l2_subdev *sd)
1289 {
1290         struct s5c73m3 *state = oif_sd_to_s5c73m3(sd);
1291
1292         v4l2_ctrl_handler_log_status(sd->ctrl_handler, sd->name);
1293
1294         v4l2_info(sd, "power: %d, apply_fmt: %d\n", state->power,
1295                                                         state->apply_fmt);
1296
1297         return 0;
1298 }
1299
1300 static int s5c73m3_open(struct v4l2_subdev *sd, struct v4l2_subdev_fh *fh)
1301 {
1302         struct v4l2_mbus_framefmt *mf;
1303
1304         mf = v4l2_subdev_get_try_format(fh, S5C73M3_ISP_PAD);
1305         s5c73m3_fill_mbus_fmt(mf, &s5c73m3_isp_resolutions[1],
1306                                                 S5C73M3_ISP_FMT);
1307
1308         mf = v4l2_subdev_get_try_format(fh, S5C73M3_JPEG_PAD);
1309         s5c73m3_fill_mbus_fmt(mf, &s5c73m3_jpeg_resolutions[1],
1310                                         S5C73M3_JPEG_FMT);
1311
1312         return 0;
1313 }
1314
1315 static int s5c73m3_oif_open(struct v4l2_subdev *sd, struct v4l2_subdev_fh *fh)
1316 {
1317         struct v4l2_mbus_framefmt *mf;
1318
1319         mf = v4l2_subdev_get_try_format(fh, OIF_ISP_PAD);
1320         s5c73m3_fill_mbus_fmt(mf, &s5c73m3_isp_resolutions[1],
1321                                                 S5C73M3_ISP_FMT);
1322
1323         mf = v4l2_subdev_get_try_format(fh, OIF_JPEG_PAD);
1324         s5c73m3_fill_mbus_fmt(mf, &s5c73m3_jpeg_resolutions[1],
1325                                         S5C73M3_JPEG_FMT);
1326
1327         mf = v4l2_subdev_get_try_format(fh, OIF_SOURCE_PAD);
1328         s5c73m3_fill_mbus_fmt(mf, &s5c73m3_isp_resolutions[1],
1329                                                 S5C73M3_ISP_FMT);
1330         return 0;
1331 }
1332
1333 static int s5c73m3_gpio_set_value(struct s5c73m3 *priv, int id, u32 val)
1334 {
1335         if (!gpio_is_valid(priv->gpio[id].gpio))
1336                 return 0;
1337         gpio_set_value(priv->gpio[id].gpio, !!val);
1338         return 1;
1339 }
1340
1341 static int s5c73m3_gpio_assert(struct s5c73m3 *priv, int id)
1342 {
1343         return s5c73m3_gpio_set_value(priv, id, priv->gpio[id].level);
1344 }
1345
1346 static int s5c73m3_gpio_deassert(struct s5c73m3 *priv, int id)
1347 {
1348         return s5c73m3_gpio_set_value(priv, id, !priv->gpio[id].level);
1349 }
1350
1351 static int __s5c73m3_power_on(struct s5c73m3 *state)
1352 {
1353         int i, ret;
1354
1355         for (i = 0; i < S5C73M3_MAX_SUPPLIES; i++) {
1356                 ret = regulator_enable(state->supplies[i].consumer);
1357                 if (ret)
1358                         goto err;
1359         }
1360
1361         s5c73m3_gpio_deassert(state, STBY);
1362         usleep_range(100, 200);
1363
1364         s5c73m3_gpio_deassert(state, RST);
1365         usleep_range(50, 100);
1366
1367         return 0;
1368 err:
1369         for (--i; i >= 0; i--)
1370                 regulator_disable(state->supplies[i].consumer);
1371         return ret;
1372 }
1373
1374 static int __s5c73m3_power_off(struct s5c73m3 *state)
1375 {
1376         int i, ret;
1377
1378         if (s5c73m3_gpio_assert(state, RST))
1379                 usleep_range(10, 50);
1380
1381         if (s5c73m3_gpio_assert(state, STBY))
1382                 usleep_range(100, 200);
1383         state->streaming = 0;
1384         state->isp_ready = 0;
1385
1386         for (i = S5C73M3_MAX_SUPPLIES - 1; i >= 0; i--) {
1387                 ret = regulator_disable(state->supplies[i].consumer);
1388                 if (ret)
1389                         goto err;
1390         }
1391         return 0;
1392 err:
1393         for (++i; i < S5C73M3_MAX_SUPPLIES; i++) {
1394                 int r = regulator_enable(state->supplies[i].consumer);
1395                 if (r < 0)
1396                         v4l2_err(&state->oif_sd, "Failed to reenable %s: %d\n",
1397                                  state->supplies[i].supply, r);
1398         }
1399         return ret;
1400 }
1401
1402 static int s5c73m3_oif_set_power(struct v4l2_subdev *sd, int on)
1403 {
1404         struct s5c73m3 *state = oif_sd_to_s5c73m3(sd);
1405         int ret = 0;
1406
1407         mutex_lock(&state->lock);
1408
1409         if (on && !state->power) {
1410                 ret = __s5c73m3_power_on(state);
1411                 if (!ret)
1412                         ret = s5c73m3_isp_init(state);
1413                 if (!ret) {
1414                         state->apply_fiv = 1;
1415                         state->apply_fmt = 1;
1416                 }
1417         } else if (!on == state->power) {
1418                 ret = s5c73m3_set_af_softlanding(state);
1419                 if (!ret)
1420                         ret = __s5c73m3_power_off(state);
1421                 else
1422                         v4l2_err(sd, "Soft landing lens failed\n");
1423         }
1424         if (!ret)
1425                 state->power += on ? 1 : -1;
1426
1427         v4l2_dbg(1, s5c73m3_dbg, sd, "%s: power: %d\n",
1428                  __func__, state->power);
1429
1430         mutex_unlock(&state->lock);
1431         return ret;
1432 }
1433
1434 static int s5c73m3_oif_registered(struct v4l2_subdev *sd)
1435 {
1436         struct s5c73m3 *state = oif_sd_to_s5c73m3(sd);
1437         int ret;
1438
1439         ret = v4l2_device_register_subdev(sd->v4l2_dev, &state->sensor_sd);
1440         if (ret) {
1441                 v4l2_err(sd->v4l2_dev, "Failed to register %s\n",
1442                                                         state->oif_sd.name);
1443                 return ret;
1444         }
1445
1446         ret = media_entity_create_link(&state->sensor_sd.entity,
1447                         S5C73M3_ISP_PAD, &state->oif_sd.entity, OIF_ISP_PAD,
1448                         MEDIA_LNK_FL_IMMUTABLE | MEDIA_LNK_FL_ENABLED);
1449
1450         ret = media_entity_create_link(&state->sensor_sd.entity,
1451                         S5C73M3_JPEG_PAD, &state->oif_sd.entity, OIF_JPEG_PAD,
1452                         MEDIA_LNK_FL_IMMUTABLE | MEDIA_LNK_FL_ENABLED);
1453
1454         mutex_lock(&state->lock);
1455         ret = __s5c73m3_power_on(state);
1456         if (ret == 0)
1457                 s5c73m3_get_fw_version(state);
1458
1459         __s5c73m3_power_off(state);
1460         mutex_unlock(&state->lock);
1461
1462         v4l2_dbg(1, s5c73m3_dbg, sd, "%s: Booting %s (%d)\n",
1463                  __func__, ret ? "failed" : "succeeded", ret);
1464
1465         return ret;
1466 }
1467
1468 static void s5c73m3_oif_unregistered(struct v4l2_subdev *sd)
1469 {
1470         struct s5c73m3 *state = oif_sd_to_s5c73m3(sd);
1471         v4l2_device_unregister_subdev(&state->sensor_sd);
1472 }
1473
1474 static const struct v4l2_subdev_internal_ops s5c73m3_internal_ops = {
1475         .open           = s5c73m3_open,
1476 };
1477
1478 static const struct v4l2_subdev_pad_ops s5c73m3_pad_ops = {
1479         .enum_mbus_code         = s5c73m3_enum_mbus_code,
1480         .enum_frame_size        = s5c73m3_enum_frame_size,
1481         .get_fmt                = s5c73m3_get_fmt,
1482         .set_fmt                = s5c73m3_set_fmt,
1483 };
1484
1485 static const struct v4l2_subdev_ops s5c73m3_subdev_ops = {
1486         .pad    = &s5c73m3_pad_ops,
1487 };
1488
1489 static const struct v4l2_subdev_internal_ops oif_internal_ops = {
1490         .registered     = s5c73m3_oif_registered,
1491         .unregistered   = s5c73m3_oif_unregistered,
1492         .open           = s5c73m3_oif_open,
1493 };
1494
1495 static const struct v4l2_subdev_pad_ops s5c73m3_oif_pad_ops = {
1496         .enum_mbus_code         = s5c73m3_oif_enum_mbus_code,
1497         .enum_frame_size        = s5c73m3_oif_enum_frame_size,
1498         .enum_frame_interval    = s5c73m3_oif_enum_frame_interval,
1499         .get_fmt                = s5c73m3_oif_get_fmt,
1500         .set_fmt                = s5c73m3_oif_set_fmt,
1501         .get_frame_desc         = s5c73m3_oif_get_frame_desc,
1502         .set_frame_desc         = s5c73m3_oif_set_frame_desc,
1503 };
1504
1505 static const struct v4l2_subdev_core_ops s5c73m3_oif_core_ops = {
1506         .s_power        = s5c73m3_oif_set_power,
1507         .log_status     = s5c73m3_oif_log_status,
1508 };
1509
1510 static const struct v4l2_subdev_video_ops s5c73m3_oif_video_ops = {
1511         .s_stream               = s5c73m3_oif_s_stream,
1512         .g_frame_interval       = s5c73m3_oif_g_frame_interval,
1513         .s_frame_interval       = s5c73m3_oif_s_frame_interval,
1514 };
1515
1516 static const struct v4l2_subdev_ops oif_subdev_ops = {
1517         .core   = &s5c73m3_oif_core_ops,
1518         .pad    = &s5c73m3_oif_pad_ops,
1519         .video  = &s5c73m3_oif_video_ops,
1520 };
1521
1522 static int s5c73m3_configure_gpios(struct s5c73m3 *state,
1523                                    const struct s5c73m3_platform_data *pdata)
1524 {
1525         struct device *dev = &state->i2c_client->dev;
1526         const struct s5c73m3_gpio *gpio;
1527         unsigned long flags;
1528         int ret;
1529
1530         state->gpio[STBY].gpio = -EINVAL;
1531         state->gpio[RST].gpio  = -EINVAL;
1532
1533         gpio = &pdata->gpio_stby;
1534         if (gpio_is_valid(gpio->gpio)) {
1535                 flags = (gpio->level ? GPIOF_OUT_INIT_HIGH : GPIOF_OUT_INIT_LOW)
1536                       | GPIOF_EXPORT;
1537                 ret = devm_gpio_request_one(dev, gpio->gpio, flags,
1538                                             "S5C73M3_STBY");
1539                 if (ret < 0)
1540                         return ret;
1541
1542                 state->gpio[STBY] = *gpio;
1543         }
1544
1545         gpio = &pdata->gpio_reset;
1546         if (gpio_is_valid(gpio->gpio)) {
1547                 flags = (gpio->level ? GPIOF_OUT_INIT_HIGH : GPIOF_OUT_INIT_LOW)
1548                       | GPIOF_EXPORT;
1549                 ret = devm_gpio_request_one(dev, gpio->gpio, flags,
1550                                             "S5C73M3_RST");
1551                 if (ret < 0)
1552                         return ret;
1553
1554                 state->gpio[RST] = *gpio;
1555         }
1556
1557         return 0;
1558 }
1559
1560 static int s5c73m3_probe(struct i2c_client *client,
1561                                 const struct i2c_device_id *id)
1562 {
1563         struct device *dev = &client->dev;
1564         const struct s5c73m3_platform_data *pdata = client->dev.platform_data;
1565         struct v4l2_subdev *sd;
1566         struct v4l2_subdev *oif_sd;
1567         struct s5c73m3 *state;
1568         int ret, i;
1569
1570         if (pdata == NULL) {
1571                 dev_err(&client->dev, "Platform data not specified\n");
1572                 return -EINVAL;
1573         }
1574
1575         state = devm_kzalloc(dev, sizeof(*state), GFP_KERNEL);
1576         if (!state)
1577                 return -ENOMEM;
1578
1579         mutex_init(&state->lock);
1580         sd = &state->sensor_sd;
1581         oif_sd = &state->oif_sd;
1582
1583         v4l2_subdev_init(sd, &s5c73m3_subdev_ops);
1584         sd->owner = client->dev.driver->owner;
1585         v4l2_set_subdevdata(sd, state);
1586         strlcpy(sd->name, "S5C73M3", sizeof(sd->name));
1587
1588         sd->internal_ops = &s5c73m3_internal_ops;
1589         sd->flags |= V4L2_SUBDEV_FL_HAS_DEVNODE;
1590
1591         state->sensor_pads[S5C73M3_JPEG_PAD].flags = MEDIA_PAD_FL_SOURCE;
1592         state->sensor_pads[S5C73M3_ISP_PAD].flags = MEDIA_PAD_FL_SOURCE;
1593         sd->entity.type = MEDIA_ENT_T_V4L2_SUBDEV;
1594
1595         ret = media_entity_init(&sd->entity, S5C73M3_NUM_PADS,
1596                                                         state->sensor_pads, 0);
1597         if (ret < 0)
1598                 return ret;
1599
1600         v4l2_i2c_subdev_init(oif_sd, client, &oif_subdev_ops);
1601         strcpy(oif_sd->name, "S5C73M3-OIF");
1602
1603         oif_sd->internal_ops = &oif_internal_ops;
1604         oif_sd->flags |= V4L2_SUBDEV_FL_HAS_DEVNODE;
1605
1606         state->oif_pads[OIF_ISP_PAD].flags = MEDIA_PAD_FL_SINK;
1607         state->oif_pads[OIF_JPEG_PAD].flags = MEDIA_PAD_FL_SINK;
1608         state->oif_pads[OIF_SOURCE_PAD].flags = MEDIA_PAD_FL_SOURCE;
1609         oif_sd->entity.type = MEDIA_ENT_T_V4L2_SUBDEV;
1610
1611         ret = media_entity_init(&oif_sd->entity, OIF_NUM_PADS,
1612                                                         state->oif_pads, 0);
1613         if (ret < 0)
1614                 return ret;
1615
1616         state->mclk_frequency = pdata->mclk_frequency;
1617         state->bus_type = pdata->bus_type;
1618         state->i2c_client = client;
1619
1620         ret = s5c73m3_configure_gpios(state, pdata);
1621         if (ret)
1622                 goto out_err;
1623
1624         for (i = 0; i < S5C73M3_MAX_SUPPLIES; i++)
1625                 state->supplies[i].supply = s5c73m3_supply_names[i];
1626
1627         ret = devm_regulator_bulk_get(dev, S5C73M3_MAX_SUPPLIES,
1628                                state->supplies);
1629         if (ret) {
1630                 dev_err(dev, "failed to get regulators\n");
1631                 goto out_err;
1632         }
1633
1634         ret = s5c73m3_init_controls(state);
1635         if (ret)
1636                 goto out_err;
1637
1638         state->sensor_pix_size[RES_ISP] = &s5c73m3_isp_resolutions[1];
1639         state->sensor_pix_size[RES_JPEG] = &s5c73m3_jpeg_resolutions[1];
1640         state->oif_pix_size[RES_ISP] = state->sensor_pix_size[RES_ISP];
1641         state->oif_pix_size[RES_JPEG] = state->sensor_pix_size[RES_JPEG];
1642
1643         state->mbus_code = S5C73M3_ISP_FMT;
1644
1645         state->fiv = &s5c73m3_intervals[S5C73M3_DEFAULT_FRAME_INTERVAL];
1646
1647         state->fw_file_version[0] = 'G';
1648         state->fw_file_version[1] = 'C';
1649
1650         ret = s5c73m3_register_spi_driver(state);
1651         if (ret < 0)
1652                 goto out_err;
1653
1654         v4l2_info(sd, "%s: completed successfully\n", __func__);
1655         return 0;
1656
1657 out_err:
1658         media_entity_cleanup(&sd->entity);
1659         return ret;
1660 }
1661
1662 static int s5c73m3_remove(struct i2c_client *client)
1663 {
1664         struct v4l2_subdev *oif_sd = i2c_get_clientdata(client);
1665         struct s5c73m3 *state = oif_sd_to_s5c73m3(oif_sd);
1666         struct v4l2_subdev *sensor_sd = &state->sensor_sd;
1667
1668         v4l2_device_unregister_subdev(oif_sd);
1669
1670         v4l2_ctrl_handler_free(oif_sd->ctrl_handler);
1671         media_entity_cleanup(&oif_sd->entity);
1672
1673         v4l2_device_unregister_subdev(sensor_sd);
1674         media_entity_cleanup(&sensor_sd->entity);
1675
1676         s5c73m3_unregister_spi_driver(state);
1677
1678         return 0;
1679 }
1680
1681 static const struct i2c_device_id s5c73m3_id[] = {
1682         { DRIVER_NAME, 0 },
1683         { }
1684 };
1685 MODULE_DEVICE_TABLE(i2c, s5c73m3_id);
1686
1687 static struct i2c_driver s5c73m3_i2c_driver = {
1688         .driver = {
1689                 .name   = DRIVER_NAME,
1690         },
1691         .probe          = s5c73m3_probe,
1692         .remove         = s5c73m3_remove,
1693         .id_table       = s5c73m3_id,
1694 };
1695
1696 module_i2c_driver(s5c73m3_i2c_driver);
1697
1698 MODULE_DESCRIPTION("Samsung S5C73M3 camera driver");
1699 MODULE_AUTHOR("Sylwester Nawrocki <s.nawrocki@samsung.com>");
1700 MODULE_LICENSE("GPL");