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