git.maquefel.me Git - linux.git/blob

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