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

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