kernel/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_pad_config *cfg,
 828                                    struct v4l2_subdev_format *fmt,
 829                                    const struct s5c73m3_frame_size **fs)
 830 {
 831         struct v4l2_subdev *sd = &state->sensor_sd;
 832         u32 code;
 833
 834         switch (fmt->pad) {
 835         case OIF_ISP_PAD:
 836                 *fs = s5c73m3_find_frame_size(&fmt->format, RES_ISP);
 837                 code = S5C73M3_ISP_FMT;
 838                 break;
 839         case OIF_JPEG_PAD:
 840                 *fs = s5c73m3_find_frame_size(&fmt->format, RES_JPEG);
 841                 code = S5C73M3_JPEG_FMT;
 842                 break;
 843         case OIF_SOURCE_PAD:
 844         default:
 845                 if (fmt->format.code == S5C73M3_JPEG_FMT)
 846                         code = S5C73M3_JPEG_FMT;
 847                 else
 848                         code = S5C73M3_ISP_FMT;
 849
 850                 if (fmt->which == V4L2_SUBDEV_FORMAT_ACTIVE)
 851                         *fs = state->oif_pix_size[RES_ISP];
 852                 else
 853                         *fs = s5c73m3_find_frame_size(
 854                                                 v4l2_subdev_get_try_format(sd, cfg,
 855                                                         OIF_ISP_PAD),
 856                                                 RES_ISP);
 857                 break;
 858         }
 859
 860         s5c73m3_fill_mbus_fmt(&fmt->format, *fs, code);
 861 }
 862
 863 static void s5c73m3_try_format(struct s5c73m3 *state,
 864                               struct v4l2_subdev_pad_config *cfg,
 865                               struct v4l2_subdev_format *fmt,
 866                               const struct s5c73m3_frame_size **fs)
 867 {
 868         u32 code;
 869
 870         if (fmt->pad == S5C73M3_ISP_PAD) {
 871                 *fs = s5c73m3_find_frame_size(&fmt->format, RES_ISP);
 872                 code = S5C73M3_ISP_FMT;
 873         } else {
 874                 *fs = s5c73m3_find_frame_size(&fmt->format, RES_JPEG);
 875                 code = S5C73M3_JPEG_FMT;
 876         }
 877
 878         s5c73m3_fill_mbus_fmt(&fmt->format, *fs, code);
 879 }
 880
 881 static int s5c73m3_oif_g_frame_interval(struct v4l2_subdev *sd,
 882                                    struct v4l2_subdev_frame_interval *fi)
 883 {
 884         struct s5c73m3 *state = oif_sd_to_s5c73m3(sd);
 885
 886         if (fi->pad != OIF_SOURCE_PAD)
 887                 return -EINVAL;
 888
 889         mutex_lock(&state->lock);
 890         fi->interval = state->fiv->interval;
 891         mutex_unlock(&state->lock);
 892
 893         return 0;
 894 }
 895
 896 static int __s5c73m3_set_frame_interval(struct s5c73m3 *state,
 897                                         struct v4l2_subdev_frame_interval *fi)
 898 {
 899         const struct s5c73m3_frame_size *prev_size =
 900                                                 state->sensor_pix_size[RES_ISP];
 901         const struct s5c73m3_interval *fiv = &s5c73m3_intervals[0];
 902         unsigned int ret, min_err = UINT_MAX;
 903         unsigned int i, fr_time;
 904
 905         if (fi->interval.denominator == 0)
 906                 return -EINVAL;
 907
 908         fr_time = fi->interval.numerator * 1000 / fi->interval.denominator;
 909
 910         for (i = 0; i < ARRAY_SIZE(s5c73m3_intervals); i++) {
 911                 const struct s5c73m3_interval *iv = &s5c73m3_intervals[i];
 912
 913                 if (prev_size->width > iv->size.width ||
 914                     prev_size->height > iv->size.height)
 915                         continue;
 916
 917                 ret = abs(iv->interval.numerator / 1000 - fr_time);
 918                 if (ret < min_err) {
 919                         fiv = iv;
 920                         min_err = ret;
 921                 }
 922         }
 923         state->fiv = fiv;
 924
 925         v4l2_dbg(1, s5c73m3_dbg, &state->sensor_sd,
 926                  "Changed frame interval to %u us\n", fiv->interval.numerator);
 927         return 0;
 928 }
 929
 930 static int s5c73m3_oif_s_frame_interval(struct v4l2_subdev *sd,
 931                                    struct v4l2_subdev_frame_interval *fi)
 932 {
 933         struct s5c73m3 *state = oif_sd_to_s5c73m3(sd);
 934         int ret;
 935
 936         if (fi->pad != OIF_SOURCE_PAD)
 937                 return -EINVAL;
 938
 939         v4l2_dbg(1, s5c73m3_dbg, sd, "Setting %d/%d frame interval\n",
 940                  fi->interval.numerator, fi->interval.denominator);
 941
 942         mutex_lock(&state->lock);
 943
 944         ret = __s5c73m3_set_frame_interval(state, fi);
 945         if (!ret) {
 946                 if (state->streaming)
 947                         ret = s5c73m3_set_frame_rate(state);
 948                 else
 949                         state->apply_fiv = 1;
 950         }
 951         mutex_unlock(&state->lock);
 952         return ret;
 953 }
 954
 955 static int s5c73m3_oif_enum_frame_interval(struct v4l2_subdev *sd,
 956                               struct v4l2_subdev_pad_config *cfg,
 957                               struct v4l2_subdev_frame_interval_enum *fie)
 958 {
 959         struct s5c73m3 *state = oif_sd_to_s5c73m3(sd);
 960         const struct s5c73m3_interval *fi;
 961         int ret = 0;
 962
 963         if (fie->pad != OIF_SOURCE_PAD)
 964                 return -EINVAL;
 965         if (fie->index >= ARRAY_SIZE(s5c73m3_intervals))
 966                 return -EINVAL;
 967
 968         mutex_lock(&state->lock);
 969         fi = &s5c73m3_intervals[fie->index];
 970         if (fie->width > fi->size.width || fie->height > fi->size.height)
 971                 ret = -EINVAL;
 972         else
 973                 fie->interval = fi->interval;
 974         mutex_unlock(&state->lock);
 975
 976         return ret;
 977 }
 978
 979 static int s5c73m3_oif_get_pad_code(int pad, int index)
 980 {
 981         if (pad == OIF_SOURCE_PAD) {
 982                 if (index > 1)
 983                         return -EINVAL;
 984                 return (index == 0) ? S5C73M3_ISP_FMT : S5C73M3_JPEG_FMT;
 985         }
 986
 987         if (index > 0)
 988                 return -EINVAL;
 989
 990         return (pad == OIF_ISP_PAD) ? S5C73M3_ISP_FMT : S5C73M3_JPEG_FMT;
 991 }
 992
 993 static int s5c73m3_get_fmt(struct v4l2_subdev *sd,
 994                            struct v4l2_subdev_pad_config *cfg,
 995                            struct v4l2_subdev_format *fmt)
 996 {
 997         struct s5c73m3 *state = sensor_sd_to_s5c73m3(sd);
 998         const struct s5c73m3_frame_size *fs;
 999         u32 code;
1000
1001         if (fmt->which == V4L2_SUBDEV_FORMAT_TRY) {
1002                 fmt->format = *v4l2_subdev_get_try_format(sd, cfg, fmt->pad);
1003                 return 0;
1004         }
1005
1006         mutex_lock(&state->lock);
1007
1008         switch (fmt->pad) {
1009         case S5C73M3_ISP_PAD:
1010                 code = S5C73M3_ISP_FMT;
1011                 fs = state->sensor_pix_size[RES_ISP];
1012                 break;
1013         case S5C73M3_JPEG_PAD:
1014                 code = S5C73M3_JPEG_FMT;
1015                 fs = state->sensor_pix_size[RES_JPEG];
1016                 break;
1017         default:
1018                 mutex_unlock(&state->lock);
1019                 return -EINVAL;
1020         }
1021         s5c73m3_fill_mbus_fmt(&fmt->format, fs, code);
1022
1023         mutex_unlock(&state->lock);
1024         return 0;
1025 }
1026
1027 static int s5c73m3_oif_get_fmt(struct v4l2_subdev *sd,
1028                            struct v4l2_subdev_pad_config *cfg,
1029                            struct v4l2_subdev_format *fmt)
1030 {
1031         struct s5c73m3 *state = oif_sd_to_s5c73m3(sd);
1032         const struct s5c73m3_frame_size *fs;
1033         u32 code;
1034
1035         if (fmt->which == V4L2_SUBDEV_FORMAT_TRY) {
1036                 fmt->format = *v4l2_subdev_get_try_format(sd, cfg, fmt->pad);
1037                 return 0;
1038         }
1039
1040         mutex_lock(&state->lock);
1041
1042         switch (fmt->pad) {
1043         case OIF_ISP_PAD:
1044                 code = S5C73M3_ISP_FMT;
1045                 fs = state->oif_pix_size[RES_ISP];
1046                 break;
1047         case OIF_JPEG_PAD:
1048                 code = S5C73M3_JPEG_FMT;
1049                 fs = state->oif_pix_size[RES_JPEG];
1050                 break;
1051         case OIF_SOURCE_PAD:
1052                 code = state->mbus_code;
1053                 fs = state->oif_pix_size[RES_ISP];
1054                 break;
1055         default:
1056                 mutex_unlock(&state->lock);
1057                 return -EINVAL;
1058         }
1059         s5c73m3_fill_mbus_fmt(&fmt->format, fs, code);
1060
1061         mutex_unlock(&state->lock);
1062         return 0;
1063 }
1064
1065 static int s5c73m3_set_fmt(struct v4l2_subdev *sd,
1066                            struct v4l2_subdev_pad_config *cfg,
1067                            struct v4l2_subdev_format *fmt)
1068 {
1069         const struct s5c73m3_frame_size *frame_size = NULL;
1070         struct s5c73m3 *state = sensor_sd_to_s5c73m3(sd);
1071         struct v4l2_mbus_framefmt *mf;
1072         int ret = 0;
1073
1074         mutex_lock(&state->lock);
1075
1076         s5c73m3_try_format(state, cfg, fmt, &frame_size);
1077
1078         if (fmt->which == V4L2_SUBDEV_FORMAT_TRY) {
1079                 mf = v4l2_subdev_get_try_format(sd, cfg, fmt->pad);
1080                 *mf = fmt->format;
1081         } else {
1082                 switch (fmt->pad) {
1083                 case S5C73M3_ISP_PAD:
1084                         state->sensor_pix_size[RES_ISP] = frame_size;
1085                         break;
1086                 case S5C73M3_JPEG_PAD:
1087                         state->sensor_pix_size[RES_JPEG] = frame_size;
1088                         break;
1089                 default:
1090                         ret = -EBUSY;
1091                 }
1092
1093                 if (state->streaming)
1094                         ret = -EBUSY;
1095                 else
1096                         state->apply_fmt = 1;
1097         }
1098
1099         mutex_unlock(&state->lock);
1100
1101         return ret;
1102 }
1103
1104 static int s5c73m3_oif_set_fmt(struct v4l2_subdev *sd,
1105                          struct v4l2_subdev_pad_config *cfg,
1106                          struct v4l2_subdev_format *fmt)
1107 {
1108         const struct s5c73m3_frame_size *frame_size = NULL;
1109         struct s5c73m3 *state = oif_sd_to_s5c73m3(sd);
1110         struct v4l2_mbus_framefmt *mf;
1111         int ret = 0;
1112
1113         mutex_lock(&state->lock);
1114
1115         s5c73m3_oif_try_format(state, cfg, fmt, &frame_size);
1116
1117         if (fmt->which == V4L2_SUBDEV_FORMAT_TRY) {
1118                 mf = v4l2_subdev_get_try_format(sd, cfg, fmt->pad);
1119                 *mf = fmt->format;
1120                 if (fmt->pad == OIF_ISP_PAD) {
1121                         mf = v4l2_subdev_get_try_format(sd, cfg, OIF_SOURCE_PAD);
1122                         mf->width = fmt->format.width;
1123                         mf->height = fmt->format.height;
1124                 }
1125         } else {
1126                 switch (fmt->pad) {
1127                 case OIF_ISP_PAD:
1128                         state->oif_pix_size[RES_ISP] = frame_size;
1129                         break;
1130                 case OIF_JPEG_PAD:
1131                         state->oif_pix_size[RES_JPEG] = frame_size;
1132                         break;
1133                 case OIF_SOURCE_PAD:
1134                         state->mbus_code = fmt->format.code;
1135                         break;
1136                 default:
1137                         ret = -EBUSY;
1138                 }
1139
1140                 if (state->streaming)
1141                         ret = -EBUSY;
1142                 else
1143                         state->apply_fmt = 1;
1144         }
1145
1146         mutex_unlock(&state->lock);
1147
1148         return ret;
1149 }
1150
1151 static int s5c73m3_oif_get_frame_desc(struct v4l2_subdev *sd, unsigned int pad,
1152                                   struct v4l2_mbus_frame_desc *fd)
1153 {
1154         struct s5c73m3 *state = oif_sd_to_s5c73m3(sd);
1155         int i;
1156
1157         if (pad != OIF_SOURCE_PAD || fd == NULL)
1158                 return -EINVAL;
1159
1160         mutex_lock(&state->lock);
1161         fd->num_entries = 2;
1162         for (i = 0; i < fd->num_entries; i++)
1163                 fd->entry[i] = state->frame_desc.entry[i];
1164         mutex_unlock(&state->lock);
1165
1166         return 0;
1167 }
1168
1169 static int s5c73m3_oif_set_frame_desc(struct v4l2_subdev *sd, unsigned int pad,
1170                                       struct v4l2_mbus_frame_desc *fd)
1171 {
1172         struct s5c73m3 *state = oif_sd_to_s5c73m3(sd);
1173         struct v4l2_mbus_frame_desc *frame_desc = &state->frame_desc;
1174         int i;
1175
1176         if (pad != OIF_SOURCE_PAD || fd == NULL)
1177                 return -EINVAL;
1178
1179         fd->entry[0].length = 10 * SZ_1M;
1180         fd->entry[1].length = max_t(u32, fd->entry[1].length,
1181                                     S5C73M3_EMBEDDED_DATA_MAXLEN);
1182         fd->num_entries = 2;
1183
1184         mutex_lock(&state->lock);
1185         for (i = 0; i < fd->num_entries; i++)
1186                 frame_desc->entry[i] = fd->entry[i];
1187         mutex_unlock(&state->lock);
1188
1189         return 0;
1190 }
1191
1192 static int s5c73m3_enum_mbus_code(struct v4l2_subdev *sd,
1193                                   struct v4l2_subdev_pad_config *cfg,
1194                                   struct v4l2_subdev_mbus_code_enum *code)
1195 {
1196         static const int codes[] = {
1197                         [S5C73M3_ISP_PAD] = S5C73M3_ISP_FMT,
1198                         [S5C73M3_JPEG_PAD] = S5C73M3_JPEG_FMT};
1199
1200         if (code->index > 0 || code->pad >= S5C73M3_NUM_PADS)
1201                 return -EINVAL;
1202
1203         code->code = codes[code->pad];
1204
1205         return 0;
1206 }
1207
1208 static int s5c73m3_oif_enum_mbus_code(struct v4l2_subdev *sd,
1209                                 struct v4l2_subdev_pad_config *cfg,
1210                                 struct v4l2_subdev_mbus_code_enum *code)
1211 {
1212         int ret;
1213
1214         ret = s5c73m3_oif_get_pad_code(code->pad, code->index);
1215         if (ret < 0)
1216                 return ret;
1217
1218         code->code = ret;
1219
1220         return 0;
1221 }
1222
1223 static int s5c73m3_enum_frame_size(struct v4l2_subdev *sd,
1224                                    struct v4l2_subdev_pad_config *cfg,
1225                                    struct v4l2_subdev_frame_size_enum *fse)
1226 {
1227         int idx;
1228
1229         if (fse->pad == S5C73M3_ISP_PAD) {
1230                 if (fse->code != S5C73M3_ISP_FMT)
1231                         return -EINVAL;
1232                 idx = RES_ISP;
1233         } else{
1234                 if (fse->code != S5C73M3_JPEG_FMT)
1235                         return -EINVAL;
1236                 idx = RES_JPEG;
1237         }
1238
1239         if (fse->index >= s5c73m3_resolutions_len[idx])
1240                 return -EINVAL;
1241
1242         fse->min_width  = s5c73m3_resolutions[idx][fse->index].width;
1243         fse->max_width  = fse->min_width;
1244         fse->max_height = s5c73m3_resolutions[idx][fse->index].height;
1245         fse->min_height = fse->max_height;
1246
1247         return 0;
1248 }
1249
1250 static int s5c73m3_oif_enum_frame_size(struct v4l2_subdev *sd,
1251                                    struct v4l2_subdev_pad_config *cfg,
1252                                    struct v4l2_subdev_frame_size_enum *fse)
1253 {
1254         struct s5c73m3 *state = oif_sd_to_s5c73m3(sd);
1255         int idx;
1256
1257         if (fse->pad == OIF_SOURCE_PAD) {
1258                 if (fse->index > 0)
1259                         return -EINVAL;
1260
1261                 switch (fse->code) {
1262                 case S5C73M3_JPEG_FMT:
1263                 case S5C73M3_ISP_FMT: {
1264                         unsigned w, h;
1265
1266                         if (fse->which == V4L2_SUBDEV_FORMAT_TRY) {
1267                                 struct v4l2_mbus_framefmt *mf;
1268
1269                                 mf = v4l2_subdev_get_try_format(sd, cfg,
1270                                                                 OIF_ISP_PAD);
1271
1272                                 w = mf->width;
1273                                 h = mf->height;
1274                         } else {
1275                                 const struct s5c73m3_frame_size *fs;
1276
1277                                 fs = state->oif_pix_size[RES_ISP];
1278                                 w = fs->width;
1279                                 h = fs->height;
1280                         }
1281                         fse->max_width = fse->min_width = w;
1282                         fse->max_height = fse->min_height = h;
1283                         return 0;
1284                 }
1285                 default:
1286                         return -EINVAL;
1287                 }
1288         }
1289
1290         if (fse->code != s5c73m3_oif_get_pad_code(fse->pad, 0))
1291                 return -EINVAL;
1292
1293         if (fse->pad == OIF_JPEG_PAD)
1294                 idx = RES_JPEG;
1295         else
1296                 idx = RES_ISP;
1297
1298         if (fse->index >= s5c73m3_resolutions_len[idx])
1299                 return -EINVAL;
1300
1301         fse->min_width  = s5c73m3_resolutions[idx][fse->index].width;
1302         fse->max_width  = fse->min_width;
1303         fse->max_height = s5c73m3_resolutions[idx][fse->index].height;
1304         fse->min_height = fse->max_height;
1305
1306         return 0;
1307 }
1308
1309 static int s5c73m3_oif_log_status(struct v4l2_subdev *sd)
1310 {
1311         struct s5c73m3 *state = oif_sd_to_s5c73m3(sd);
1312
1313         v4l2_ctrl_handler_log_status(sd->ctrl_handler, sd->name);
1314
1315         v4l2_info(sd, "power: %d, apply_fmt: %d\n", state->power,
1316                                                         state->apply_fmt);
1317
1318         return 0;
1319 }
1320
1321 static int s5c73m3_open(struct v4l2_subdev *sd, struct v4l2_subdev_fh *fh)
1322 {
1323         struct v4l2_mbus_framefmt *mf;
1324
1325         mf = v4l2_subdev_get_try_format(sd, fh->pad, S5C73M3_ISP_PAD);
1326         s5c73m3_fill_mbus_fmt(mf, &s5c73m3_isp_resolutions[1],
1327                                                 S5C73M3_ISP_FMT);
1328
1329         mf = v4l2_subdev_get_try_format(sd, fh->pad, S5C73M3_JPEG_PAD);
1330         s5c73m3_fill_mbus_fmt(mf, &s5c73m3_jpeg_resolutions[1],
1331                                         S5C73M3_JPEG_FMT);
1332
1333         return 0;
1334 }
1335
1336 static int s5c73m3_oif_open(struct v4l2_subdev *sd, struct v4l2_subdev_fh *fh)
1337 {
1338         struct v4l2_mbus_framefmt *mf;
1339
1340         mf = v4l2_subdev_get_try_format(sd, fh->pad, OIF_ISP_PAD);
1341         s5c73m3_fill_mbus_fmt(mf, &s5c73m3_isp_resolutions[1],
1342                                                 S5C73M3_ISP_FMT);
1343
1344         mf = v4l2_subdev_get_try_format(sd, fh->pad, OIF_JPEG_PAD);
1345         s5c73m3_fill_mbus_fmt(mf, &s5c73m3_jpeg_resolutions[1],
1346                                         S5C73M3_JPEG_FMT);
1347
1348         mf = v4l2_subdev_get_try_format(sd, fh->pad, OIF_SOURCE_PAD);
1349         s5c73m3_fill_mbus_fmt(mf, &s5c73m3_isp_resolutions[1],
1350                                                 S5C73M3_ISP_FMT);
1351         return 0;
1352 }
1353
1354 static int s5c73m3_gpio_set_value(struct s5c73m3 *priv, int id, u32 val)
1355 {
1356         if (!gpio_is_valid(priv->gpio[id].gpio))
1357                 return 0;
1358         gpio_set_value(priv->gpio[id].gpio, !!val);
1359         return 1;
1360 }
1361
1362 static int s5c73m3_gpio_assert(struct s5c73m3 *priv, int id)
1363 {
1364         return s5c73m3_gpio_set_value(priv, id, priv->gpio[id].level);
1365 }
1366
1367 static int s5c73m3_gpio_deassert(struct s5c73m3 *priv, int id)
1368 {
1369         return s5c73m3_gpio_set_value(priv, id, !priv->gpio[id].level);
1370 }
1371
1372 static int __s5c73m3_power_on(struct s5c73m3 *state)
1373 {
1374         int i, ret;
1375
1376         for (i = 0; i < S5C73M3_MAX_SUPPLIES; i++) {
1377                 ret = regulator_enable(state->supplies[i].consumer);
1378                 if (ret)
1379                         goto err_reg_dis;
1380         }
1381
1382         ret = clk_set_rate(state->clock, state->mclk_frequency);
1383         if (ret < 0)
1384                 goto err_reg_dis;
1385
1386         ret = clk_prepare_enable(state->clock);
1387         if (ret < 0)
1388                 goto err_reg_dis;
1389
1390         v4l2_dbg(1, s5c73m3_dbg, &state->oif_sd, "clock frequency: %ld\n",
1391                                         clk_get_rate(state->clock));
1392
1393         s5c73m3_gpio_deassert(state, STBY);
1394         usleep_range(100, 200);
1395
1396         s5c73m3_gpio_deassert(state, RST);
1397         usleep_range(50, 100);
1398
1399         return 0;
1400
1401 err_reg_dis:
1402         for (--i; i >= 0; i--)
1403                 regulator_disable(state->supplies[i].consumer);
1404         return ret;
1405 }
1406
1407 static int __s5c73m3_power_off(struct s5c73m3 *state)
1408 {
1409         int i, ret;
1410
1411         if (s5c73m3_gpio_assert(state, RST))
1412                 usleep_range(10, 50);
1413
1414         if (s5c73m3_gpio_assert(state, STBY))
1415                 usleep_range(100, 200);
1416
1417         clk_disable_unprepare(state->clock);
1418
1419         state->streaming = 0;
1420         state->isp_ready = 0;
1421
1422         for (i = S5C73M3_MAX_SUPPLIES - 1; i >= 0; i--) {
1423                 ret = regulator_disable(state->supplies[i].consumer);
1424                 if (ret)
1425                         goto err;
1426         }
1427
1428         return 0;
1429 err:
1430         for (++i; i < S5C73M3_MAX_SUPPLIES; i++) {
1431                 int r = regulator_enable(state->supplies[i].consumer);
1432                 if (r < 0)
1433                         v4l2_err(&state->oif_sd, "Failed to reenable %s: %d\n",
1434                                  state->supplies[i].supply, r);
1435         }
1436
1437         clk_prepare_enable(state->clock);
1438         return ret;
1439 }
1440
1441 static int s5c73m3_oif_set_power(struct v4l2_subdev *sd, int on)
1442 {
1443         struct s5c73m3 *state = oif_sd_to_s5c73m3(sd);
1444         int ret = 0;
1445
1446         mutex_lock(&state->lock);
1447
1448         if (on && !state->power) {
1449                 ret = __s5c73m3_power_on(state);
1450                 if (!ret)
1451                         ret = s5c73m3_isp_init(state);
1452                 if (!ret) {
1453                         state->apply_fiv = 1;
1454                         state->apply_fmt = 1;
1455                 }
1456         } else if (!on == state->power) {
1457                 ret = s5c73m3_set_af_softlanding(state);
1458                 if (!ret)
1459                         ret = __s5c73m3_power_off(state);
1460                 else
1461                         v4l2_err(sd, "Soft landing lens failed\n");
1462         }
1463         if (!ret)
1464                 state->power += on ? 1 : -1;
1465
1466         v4l2_dbg(1, s5c73m3_dbg, sd, "%s: power: %d\n",
1467                  __func__, state->power);
1468
1469         mutex_unlock(&state->lock);
1470         return ret;
1471 }
1472
1473 static int s5c73m3_oif_registered(struct v4l2_subdev *sd)
1474 {
1475         struct s5c73m3 *state = oif_sd_to_s5c73m3(sd);
1476         int ret;
1477
1478         ret = v4l2_device_register_subdev(sd->v4l2_dev, &state->sensor_sd);
1479         if (ret) {
1480                 v4l2_err(sd->v4l2_dev, "Failed to register %s\n",
1481                                                         state->oif_sd.name);
1482                 return ret;
1483         }
1484
1485         ret = media_entity_create_link(&state->sensor_sd.entity,
1486                         S5C73M3_ISP_PAD, &state->oif_sd.entity, OIF_ISP_PAD,
1487                         MEDIA_LNK_FL_IMMUTABLE | MEDIA_LNK_FL_ENABLED);
1488
1489         ret = media_entity_create_link(&state->sensor_sd.entity,
1490                         S5C73M3_JPEG_PAD, &state->oif_sd.entity, OIF_JPEG_PAD,
1491                         MEDIA_LNK_FL_IMMUTABLE | MEDIA_LNK_FL_ENABLED);
1492
1493         return ret;
1494 }
1495
1496 static void s5c73m3_oif_unregistered(struct v4l2_subdev *sd)
1497 {
1498         struct s5c73m3 *state = oif_sd_to_s5c73m3(sd);
1499         v4l2_device_unregister_subdev(&state->sensor_sd);
1500 }
1501
1502 static const struct v4l2_subdev_internal_ops s5c73m3_internal_ops = {
1503         .open           = s5c73m3_open,
1504 };
1505
1506 static const struct v4l2_subdev_pad_ops s5c73m3_pad_ops = {
1507         .enum_mbus_code         = s5c73m3_enum_mbus_code,
1508         .enum_frame_size        = s5c73m3_enum_frame_size,
1509         .get_fmt                = s5c73m3_get_fmt,
1510         .set_fmt                = s5c73m3_set_fmt,
1511 };
1512
1513 static const struct v4l2_subdev_ops s5c73m3_subdev_ops = {
1514         .pad    = &s5c73m3_pad_ops,
1515 };
1516
1517 static const struct v4l2_subdev_internal_ops oif_internal_ops = {
1518         .registered     = s5c73m3_oif_registered,
1519         .unregistered   = s5c73m3_oif_unregistered,
1520         .open           = s5c73m3_oif_open,
1521 };
1522
1523 static const struct v4l2_subdev_pad_ops s5c73m3_oif_pad_ops = {
1524         .enum_mbus_code         = s5c73m3_oif_enum_mbus_code,
1525         .enum_frame_size        = s5c73m3_oif_enum_frame_size,
1526         .enum_frame_interval    = s5c73m3_oif_enum_frame_interval,
1527         .get_fmt                = s5c73m3_oif_get_fmt,
1528         .set_fmt                = s5c73m3_oif_set_fmt,
1529         .get_frame_desc         = s5c73m3_oif_get_frame_desc,
1530         .set_frame_desc         = s5c73m3_oif_set_frame_desc,
1531 };
1532
1533 static const struct v4l2_subdev_core_ops s5c73m3_oif_core_ops = {
1534         .s_power        = s5c73m3_oif_set_power,
1535         .log_status     = s5c73m3_oif_log_status,
1536 };
1537
1538 static const struct v4l2_subdev_video_ops s5c73m3_oif_video_ops = {
1539         .s_stream               = s5c73m3_oif_s_stream,
1540         .g_frame_interval       = s5c73m3_oif_g_frame_interval,
1541         .s_frame_interval       = s5c73m3_oif_s_frame_interval,
1542 };
1543
1544 static const struct v4l2_subdev_ops oif_subdev_ops = {
1545         .core   = &s5c73m3_oif_core_ops,
1546         .pad    = &s5c73m3_oif_pad_ops,
1547         .video  = &s5c73m3_oif_video_ops,
1548 };
1549
1550 static int s5c73m3_configure_gpios(struct s5c73m3 *state)
1551 {
1552         static const char * const gpio_names[] = {
1553                 "S5C73M3_STBY", "S5C73M3_RST"
1554         };
1555         struct i2c_client *c = state->i2c_client;
1556         struct s5c73m3_gpio *g = state->gpio;
1557         int ret, i;
1558
1559         for (i = 0; i < GPIO_NUM; ++i) {
1560                 unsigned int flags = GPIOF_DIR_OUT;
1561                 if (g[i].level)
1562                         flags |= GPIOF_INIT_HIGH;
1563                 ret = devm_gpio_request_one(&c->dev, g[i].gpio, flags,
1564                                             gpio_names[i]);
1565                 if (ret) {
1566                         v4l2_err(c, "failed to request gpio %s\n",
1567                                  gpio_names[i]);
1568                         return ret;
1569                 }
1570         }
1571         return 0;
1572 }
1573
1574 static int s5c73m3_parse_gpios(struct s5c73m3 *state)
1575 {
1576         static const char * const prop_names[] = {
1577                 "standby-gpios", "xshutdown-gpios",
1578         };
1579         struct device *dev = &state->i2c_client->dev;
1580         struct device_node *node = dev->of_node;
1581         int ret, i;
1582
1583         for (i = 0; i < GPIO_NUM; ++i) {
1584                 enum of_gpio_flags of_flags;
1585
1586                 ret = of_get_named_gpio_flags(node, prop_names[i],
1587                                               0, &of_flags);
1588                 if (ret < 0) {
1589                         dev_err(dev, "failed to parse %s DT property\n",
1590                                 prop_names[i]);
1591                         return -EINVAL;
1592                 }
1593                 state->gpio[i].gpio = ret;
1594                 state->gpio[i].level = !(of_flags & OF_GPIO_ACTIVE_LOW);
1595         }
1596         return 0;
1597 }
1598
1599 static int s5c73m3_get_platform_data(struct s5c73m3 *state)
1600 {
1601         struct device *dev = &state->i2c_client->dev;
1602         const struct s5c73m3_platform_data *pdata = dev->platform_data;
1603         struct device_node *node = dev->of_node;
1604         struct device_node *node_ep;
1605         struct v4l2_of_endpoint ep;
1606         int ret;
1607
1608         if (!node) {
1609                 if (!pdata) {
1610                         dev_err(dev, "Platform data not specified\n");
1611                         return -EINVAL;
1612                 }
1613
1614                 state->mclk_frequency = pdata->mclk_frequency;
1615                 state->gpio[STBY] = pdata->gpio_stby;
1616                 state->gpio[RST] = pdata->gpio_reset;
1617                 return 0;
1618         }
1619
1620         state->clock = devm_clk_get(dev, S5C73M3_CLK_NAME);
1621         if (IS_ERR(state->clock))
1622                 return PTR_ERR(state->clock);
1623
1624         if (of_property_read_u32(node, "clock-frequency",
1625                                  &state->mclk_frequency)) {
1626                 state->mclk_frequency = S5C73M3_DEFAULT_MCLK_FREQ;
1627                 dev_info(dev, "using default %u Hz clock frequency\n",
1628                                         state->mclk_frequency);
1629         }
1630
1631         ret = s5c73m3_parse_gpios(state);
1632         if (ret < 0)
1633                 return -EINVAL;
1634
1635         node_ep = of_graph_get_next_endpoint(node, NULL);
1636         if (!node_ep) {
1637                 dev_warn(dev, "no endpoint defined for node: %s\n",
1638                                                 node->full_name);
1639                 return 0;
1640         }
1641
1642         v4l2_of_parse_endpoint(node_ep, &ep);
1643         of_node_put(node_ep);
1644
1645         if (ep.bus_type != V4L2_MBUS_CSI2) {
1646                 dev_err(dev, "unsupported bus type\n");
1647                 return -EINVAL;
1648         }
1649         /*
1650          * Number of MIPI CSI-2 data lanes is currently not configurable,
1651          * always a default value of 4 lanes is used.
1652          */
1653         if (ep.bus.mipi_csi2.num_data_lanes != S5C73M3_MIPI_DATA_LANES)
1654                 dev_info(dev, "falling back to 4 MIPI CSI-2 data lanes\n");
1655
1656         return 0;
1657 }
1658
1659 static int s5c73m3_probe(struct i2c_client *client,
1660                                 const struct i2c_device_id *id)
1661 {
1662         struct device *dev = &client->dev;
1663         struct v4l2_subdev *sd;
1664         struct v4l2_subdev *oif_sd;
1665         struct s5c73m3 *state;
1666         int ret, i;
1667
1668         state = devm_kzalloc(dev, sizeof(*state), GFP_KERNEL);
1669         if (!state)
1670                 return -ENOMEM;
1671
1672         state->i2c_client = client;
1673         ret = s5c73m3_get_platform_data(state);
1674         if (ret < 0)
1675                 return ret;
1676
1677         mutex_init(&state->lock);
1678         sd = &state->sensor_sd;
1679         oif_sd = &state->oif_sd;
1680
1681         v4l2_subdev_init(sd, &s5c73m3_subdev_ops);
1682         sd->owner = client->dev.driver->owner;
1683         v4l2_set_subdevdata(sd, state);
1684         strlcpy(sd->name, "S5C73M3", sizeof(sd->name));
1685
1686         sd->internal_ops = &s5c73m3_internal_ops;
1687         sd->flags |= V4L2_SUBDEV_FL_HAS_DEVNODE;
1688
1689         state->sensor_pads[S5C73M3_JPEG_PAD].flags = MEDIA_PAD_FL_SOURCE;
1690         state->sensor_pads[S5C73M3_ISP_PAD].flags = MEDIA_PAD_FL_SOURCE;
1691         sd->entity.type = MEDIA_ENT_T_V4L2_SUBDEV;
1692
1693         ret = media_entity_init(&sd->entity, S5C73M3_NUM_PADS,
1694                                                         state->sensor_pads, 0);
1695         if (ret < 0)
1696                 return ret;
1697
1698         v4l2_i2c_subdev_init(oif_sd, client, &oif_subdev_ops);
1699         strcpy(oif_sd->name, "S5C73M3-OIF");
1700
1701         oif_sd->internal_ops = &oif_internal_ops;
1702         oif_sd->flags |= V4L2_SUBDEV_FL_HAS_DEVNODE;
1703
1704         state->oif_pads[OIF_ISP_PAD].flags = MEDIA_PAD_FL_SINK;
1705         state->oif_pads[OIF_JPEG_PAD].flags = MEDIA_PAD_FL_SINK;
1706         state->oif_pads[OIF_SOURCE_PAD].flags = MEDIA_PAD_FL_SOURCE;
1707         oif_sd->entity.type = MEDIA_ENT_T_V4L2_SUBDEV;
1708
1709         ret = media_entity_init(&oif_sd->entity, OIF_NUM_PADS,
1710                                                         state->oif_pads, 0);
1711         if (ret < 0)
1712                 return ret;
1713
1714         ret = s5c73m3_configure_gpios(state);
1715         if (ret)
1716                 goto out_err;
1717
1718         for (i = 0; i < S5C73M3_MAX_SUPPLIES; i++)
1719                 state->supplies[i].supply = s5c73m3_supply_names[i];
1720
1721         ret = devm_regulator_bulk_get(dev, S5C73M3_MAX_SUPPLIES,
1722                                state->supplies);
1723         if (ret) {
1724                 dev_err(dev, "failed to get regulators\n");
1725                 goto out_err;
1726         }
1727
1728         ret = s5c73m3_init_controls(state);
1729         if (ret)
1730                 goto out_err;
1731
1732         state->sensor_pix_size[RES_ISP] = &s5c73m3_isp_resolutions[1];
1733         state->sensor_pix_size[RES_JPEG] = &s5c73m3_jpeg_resolutions[1];
1734         state->oif_pix_size[RES_ISP] = state->sensor_pix_size[RES_ISP];
1735         state->oif_pix_size[RES_JPEG] = state->sensor_pix_size[RES_JPEG];
1736
1737         state->mbus_code = S5C73M3_ISP_FMT;
1738
1739         state->fiv = &s5c73m3_intervals[S5C73M3_DEFAULT_FRAME_INTERVAL];
1740
1741         state->fw_file_version[0] = 'G';
1742         state->fw_file_version[1] = 'C';
1743
1744         ret = s5c73m3_register_spi_driver(state);
1745         if (ret < 0)
1746                 goto out_err;
1747
1748         oif_sd->dev = dev;
1749
1750         ret = __s5c73m3_power_on(state);
1751         if (ret < 0)
1752                 goto out_err1;
1753
1754         ret = s5c73m3_get_fw_version(state);
1755         __s5c73m3_power_off(state);
1756
1757         if (ret < 0) {
1758                 dev_err(dev, "Device detection failed: %d\n", ret);
1759                 goto out_err1;
1760         }
1761
1762         ret = v4l2_async_register_subdev(oif_sd);
1763         if (ret < 0)
1764                 goto out_err1;
1765
1766         v4l2_info(sd, "%s: completed successfully\n", __func__);
1767         return 0;
1768
1769 out_err1:
1770         s5c73m3_unregister_spi_driver(state);
1771 out_err:
1772         media_entity_cleanup(&sd->entity);
1773         return ret;
1774 }
1775
1776 static int s5c73m3_remove(struct i2c_client *client)
1777 {
1778         struct v4l2_subdev *oif_sd = i2c_get_clientdata(client);
1779         struct s5c73m3 *state = oif_sd_to_s5c73m3(oif_sd);
1780         struct v4l2_subdev *sensor_sd = &state->sensor_sd;
1781
1782         v4l2_async_unregister_subdev(oif_sd);
1783
1784         v4l2_ctrl_handler_free(oif_sd->ctrl_handler);
1785         media_entity_cleanup(&oif_sd->entity);
1786
1787         v4l2_device_unregister_subdev(sensor_sd);
1788         media_entity_cleanup(&sensor_sd->entity);
1789
1790         s5c73m3_unregister_spi_driver(state);
1791
1792         return 0;
1793 }
1794
1795 static const struct i2c_device_id s5c73m3_id[] = {
1796         { DRIVER_NAME, 0 },
1797         { }
1798 };
1799 MODULE_DEVICE_TABLE(i2c, s5c73m3_id);
1800
1801 #ifdef CONFIG_OF
1802 static const struct of_device_id s5c73m3_of_match[] = {
1803         { .compatible = "samsung,s5c73m3" },
1804         { }
1805 };
1806 MODULE_DEVICE_TABLE(of, s5c73m3_of_match);
1807 #endif
1808
1809 static struct i2c_driver s5c73m3_i2c_driver = {
1810         .driver = {
1811                 .of_match_table = of_match_ptr(s5c73m3_of_match),
1812                 .name   = DRIVER_NAME,
1813         },
1814         .probe          = s5c73m3_probe,
1815         .remove         = s5c73m3_remove,
1816         .id_table       = s5c73m3_id,
1817 };
1818
1819 module_i2c_driver(s5c73m3_i2c_driver);
1820
1821 MODULE_DESCRIPTION("Samsung S5C73M3 camera driver");
1822 MODULE_AUTHOR("Sylwester Nawrocki <s.nawrocki@samsung.com>");
1823 MODULE_LICENSE("GPL");