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

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