drivers/media/i2c/soc_camera/rockchip/aptina_camera_module.c

   1 /*
   2  * aptina_camera_module.c
   3  *
   4  * Generic galaxycore sensor driver
   5  *
   6  * Copyright (C) 2016 Fuzhou Rockchip Electronics Co., Ltd.
   7  *
   8  * Copyright (C) 2012-2014 Intel Mobile Communications GmbH
   9  *
  10  * Copyright (C) 2008 Texas Instruments.
  11  *
  12  * This file is licensed under the terms of the GNU General Public License
  13  * version 2. This program is licensed "as is" without any warranty of any
  14  * kind, whether express or implied.
  15  *
  16  */
  17
  18 #include <linux/delay.h>
  19 #include <media/v4l2-subdev.h>
  20 #include <media/v4l2-device.h>
  21 #include <media/videobuf-core.h>
  22 #include <linux/slab.h>
  23 #include <linux/gcd.h>
  24 #include <linux/i2c.h>
  25 #include <media/v4l2-controls_rockchip.h>
  26 #include "aptina_camera_module.h"
  27
  28 #define I2C_M_WR 0
  29 #define I2C_MSG_MAX 300
  30 #define I2C_DATA_MAX (I2C_MSG_MAX * 3)
  31
  32 /* ======================================================================== */
  33
  34 struct aptina_camera_module *to_aptina_camera_module(struct v4l2_subdev *sd)
  35 {
  36         return container_of(sd, struct aptina_camera_module, sd);
  37 }
  38
  39 /* ======================================================================== */
  40
  41 void aptina_camera_module_reset(
  42         struct aptina_camera_module *cam_mod)
  43 {
  44         pltfrm_camera_module_pr_debug(&cam_mod->sd, "\n");
  45
  46         cam_mod->inited = false;
  47         cam_mod->active_config = NULL;
  48         cam_mod->update_config = true;
  49         cam_mod->frm_fmt_valid = false;
  50         cam_mod->frm_intrvl_valid = false;
  51         cam_mod->exp_config.auto_exp = false;
  52         cam_mod->exp_config.auto_gain = false;
  53         cam_mod->wb_config.auto_wb = false;
  54         cam_mod->hflip = false;
  55         cam_mod->vflip = false;
  56         cam_mod->auto_adjust_fps = true;
  57         cam_mod->rotation = 0;
  58         cam_mod->ctrl_updt = 0;
  59         cam_mod->state = APTINA_CAMERA_MODULE_POWER_OFF;
  60         cam_mod->state_before_suspend = APTINA_CAMERA_MODULE_POWER_OFF;
  61 }
  62
  63 /* ======================================================================== */
  64
  65 int aptina_read_i2c_reg(
  66         struct v4l2_subdev *sd,
  67         u16 data_length,
  68         u16 reg,
  69         u32 *val)
  70 {
  71         struct i2c_client *client = v4l2_get_subdevdata(sd);
  72         int ret = 0;
  73         struct i2c_msg msg[1];
  74         unsigned char data[2] = { 0, 0};
  75
  76         if (!client->adapter) {
  77                 pltfrm_camera_module_pr_err(sd, "client->adapter NULL\n");
  78                 return -ENODEV;
  79         }
  80
  81         msg->addr = client->addr;
  82         msg->flags = I2C_M_WR;
  83         msg->len = 2;
  84         msg->buf = data;
  85
  86         /* High byte goes out first */
  87         data[0] = (u8)((reg >> 8) & 0xff);
  88         data[1] = (u8)(reg & 0xff);
  89
  90         ret = i2c_transfer(client->adapter, msg, 1);
  91         if (ret >= 0) {
  92                 mdelay(3);
  93                 msg->flags = I2C_M_RD;
  94                 msg->len = data_length;
  95                 i2c_transfer(client->adapter, msg, 1);
  96         }
  97         if (ret >= 0) {
  98                 *val = 0;
  99                 /* High byte comes first */
 100                 if (data_length == 1)
 101                         *val = data[0];
 102                 else if (data_length == 2)
 103                         *val = data[1] + (data[0] << 8);
 104                 else
 105                 ;
 106
 107                 return 0;
 108         }
 109         pltfrm_camera_module_pr_err(sd,
 110                 "i2c read from offset 0x%08x failed with error %d\n", reg, ret);
 111         return ret;
 112 }
 113
 114 /* ======================================================================== */
 115
 116 int aptina_write_i2c_reg(
 117         struct v4l2_subdev *sd,
 118         u16 reg, u16 val)
 119 {
 120         struct i2c_client *client = v4l2_get_subdevdata(sd);
 121         int ret = 0;
 122         struct i2c_msg msg[1];
 123         unsigned char data[4] = {0, 0, 0, 0};
 124         int retries;
 125
 126         if (!client->adapter) {
 127                 pltfrm_camera_module_pr_err(sd, "client->adapter NULL\n");
 128                 return -ENODEV;
 129         }
 130
 131         for (retries = 0; retries < 5; retries++) {
 132                 msg->addr = client->addr;
 133                 msg->flags = I2C_M_WR;
 134                 msg->len = 4;
 135                 msg->buf = data;
 136
 137                 /* high byte goes out first */
 138                 data[0] = (u8)((reg >> 8) & 0xff);
 139                 data[1] = (u8)(reg & 0xff);
 140                 data[2] = (u8)((val >> 8) & 0xff);
 141                 data[3] = (u8)(val & 0xff);
 142
 143                 ret = i2c_transfer(client->adapter, msg, 1);
 144                 usleep_range(20, 50);
 145
 146                 if (ret == 1)
 147                         return 0;
 148
 149                 pltfrm_camera_module_pr_debug(sd,
 150                         "retrying I2C... %d\n", retries);
 151                 retries++;
 152                 set_current_state(TASK_UNINTERRUPTIBLE);
 153                 schedule_timeout(msecs_to_jiffies(20));
 154         }
 155
 156         pltfrm_camera_module_pr_err(sd,
 157                 "i2c write to offset 0x%08x failed with error %d\n", reg, ret);
 158         return ret;
 159 }
 160
 161 /* ======================================================================== */
 162
 163 int aptina_write_reglist(
 164         struct v4l2_subdev *sd,
 165         const struct pltfrm_camera_module_reg reglist[],
 166         int len)
 167 {
 168         struct i2c_client *client = v4l2_get_subdevdata(sd);
 169         int ret = 0;
 170         unsigned int k = 0, j = 0;
 171         int i = 0;
 172         struct i2c_msg *msg;
 173         unsigned char *data;
 174         unsigned int max_entries = len;
 175
 176         msg = kmalloc((sizeof(struct i2c_msg) * I2C_MSG_MAX), GFP_KERNEL);
 177         if (!msg)
 178                 return -ENOMEM;
 179         data = kmalloc((sizeof(unsigned char) * I2C_DATA_MAX), GFP_KERNEL);
 180         if (!data) {
 181                 kfree(msg);
 182                 return -ENOMEM;
 183         }
 184
 185         for (i = 0; i < max_entries; i++) {
 186                 switch (reglist[i].flag) {
 187                 case PLTFRM_CAMERA_MODULE_REG_TYPE_DATA:
 188                         (msg + j)->addr = client->addr;
 189                         (msg + j)->flags = I2C_M_WR;
 190                         (msg + j)->len = 4;
 191                         (msg + j)->buf = (data + k);
 192
 193                         data[k + 0] = (u8)((reglist[i].reg >> 8) & 0xFF);
 194                         data[k + 1] = (u8)(reglist[i].reg & 0xFF);
 195                         data[k + 2] = (u8)((reglist[i].val >> 8) & 0xFF);
 196                         data[k + 3] = (u8)(reglist[i].val & 0xFF);
 197
 198                         k = k + 4;
 199                         j++;
 200                         if (j == (I2C_MSG_MAX - 1)) {
 201                                 /* Bulk I2C transfer */
 202                                 pltfrm_camera_module_pr_err(sd,
 203                                         "messages transfers 1 0x%p msg %d bytes %d\n",
 204                                         msg, j, k);
 205                                 ret = i2c_transfer(client->adapter, msg, j);
 206                                 if (ret < 0) {
 207                                         pltfrm_camera_module_pr_err(sd,
 208                                                 "i2c transfer returned with err %d\n",
 209                                                 ret);
 210                                         kfree(msg);
 211                                         kfree(data);
 212                                         return ret;
 213                                 }
 214                                 j = 0;
 215                                 k = 0;
 216                                 pltfrm_camera_module_pr_debug(sd,
 217                                         "i2c_transfer return %d\n", ret);
 218                         }
 219                         break;
 220                 case PLTFRM_CAMERA_MODULE_REG_TYPE_TIMEOUT:
 221                         if (j > 0) {
 222                                 /* Bulk I2C transfer */
 223                                 pltfrm_camera_module_pr_debug(sd,
 224                                         "messages transfers 1 0x%p msg %d bytes %d\n",
 225                                         msg, j, k);
 226                                 ret = i2c_transfer(client->adapter, msg, j);
 227                                 if (ret < 0) {
 228                                         pltfrm_camera_module_pr_debug(sd,
 229                                                 "i2c transfer returned with err %d\n",
 230                                                 ret);
 231                                         kfree(msg);
 232                                         kfree(data);
 233                                         return ret;
 234                                 }
 235                                 pltfrm_camera_module_pr_debug(sd,
 236                                         "i2c_transfer return %d\n", ret);
 237                         }
 238                         mdelay(reglist[i].val);
 239                         j = 0;
 240                         k = 0;
 241                         break;
 242                 default:
 243                         pltfrm_camera_module_pr_debug(sd, "unknown command\n");
 244                         kfree(msg);
 245                         kfree(data);
 246                         return -1;
 247                 }
 248         }
 249
 250         if (j != 0) {           /*Remaining I2C message*/
 251                 pltfrm_camera_module_pr_debug(sd,
 252                         "messages transfers 1 0x%p msg %d bytes %d\n",
 253                         msg, j, k);
 254                 ret = i2c_transfer(client->adapter, msg, j);
 255                 if (ret < 0) {
 256                         pltfrm_camera_module_pr_err(sd,
 257                                 "i2c transfer returned with err %d\n", ret);
 258                         kfree(msg);
 259                         kfree(data);
 260                         return ret;
 261                 }
 262                 pltfrm_camera_module_pr_debug(sd,
 263                         "i2c_transfer return %d\n", ret);
 264         }
 265
 266         kfree(msg);
 267         kfree(data);
 268         return 0;
 269 }
 270
 271 static void aptina_camera_module_set_active_config(
 272         struct aptina_camera_module *cam_mod,
 273         struct aptina_camera_module_config *new_config)
 274 {
 275         pltfrm_camera_module_pr_debug(&cam_mod->sd, "\n");
 276
 277         if (IS_ERR_OR_NULL(new_config)) {
 278                 cam_mod->active_config = new_config;
 279                 pltfrm_camera_module_pr_debug(&cam_mod->sd,
 280                         "no active config\n");
 281         } else {
 282                 cam_mod->ctrl_updt &= APTINA_CAMERA_MODULE_CTRL_UPDT_AUTO_EXP |
 283                         APTINA_CAMERA_MODULE_CTRL_UPDT_AUTO_GAIN |
 284                         APTINA_CAMERA_MODULE_CTRL_UPDT_AUTO_WB;
 285                 if (new_config->auto_exp_enabled !=
 286                         cam_mod->exp_config.auto_exp) {
 287                         cam_mod->ctrl_updt |=
 288                                 APTINA_CAMERA_MODULE_CTRL_UPDT_AUTO_EXP;
 289                         cam_mod->exp_config.auto_exp =
 290                                 new_config->auto_exp_enabled;
 291                 }
 292                 if (new_config->auto_gain_enabled !=
 293                         cam_mod->exp_config.auto_gain) {
 294                         cam_mod->ctrl_updt |=
 295                                 APTINA_CAMERA_MODULE_CTRL_UPDT_AUTO_GAIN;
 296                         cam_mod->exp_config.auto_gain =
 297                                 new_config->auto_gain_enabled;
 298                 }
 299                 if (new_config->auto_wb_enabled !=
 300                         cam_mod->wb_config.auto_wb) {
 301                         cam_mod->ctrl_updt |=
 302                                 APTINA_CAMERA_MODULE_CTRL_UPDT_AUTO_WB;
 303                         cam_mod->wb_config.auto_wb =
 304                                 new_config->auto_wb_enabled;
 305                 }
 306                 if (new_config != cam_mod->active_config) {
 307                         cam_mod->update_config = true;
 308                         cam_mod->active_config = new_config;
 309                         pltfrm_camera_module_pr_debug(&cam_mod->sd,
 310                                 "activating config '%s'\n",
 311                                 new_config->name);
 312                 }
 313         }
 314 }
 315
 316 /* ======================================================================== */
 317
 318 static struct aptina_camera_module_config *aptina_camera_module_find_config(
 319         struct aptina_camera_module *cam_mod,
 320         struct v4l2_mbus_framefmt *fmt,
 321         struct v4l2_subdev_frame_interval *frm_intrvl)
 322 {
 323         u32 i;
 324         unsigned long gcdiv;
 325         struct v4l2_subdev_frame_interval norm_interval;
 326
 327         pltfrm_camera_module_pr_debug(&cam_mod->sd, "\n");
 328         if (!IS_ERR_OR_NULL(fmt))
 329                 pltfrm_camera_module_pr_debug(&cam_mod->sd,
 330                         "%dx%d, fmt code 0x%04x\n",
 331                         fmt->width, fmt->height, fmt->code);
 332
 333         if (!IS_ERR_OR_NULL(frm_intrvl))
 334                 pltfrm_camera_module_pr_debug(&cam_mod->sd,
 335                         "frame interval %d/%d\n",
 336                         frm_intrvl->interval.numerator,
 337                         frm_intrvl->interval.denominator);
 338
 339         for (i = 0; i < cam_mod->custom.num_configs; i++) {
 340                 if (!IS_ERR_OR_NULL(frm_intrvl)) {
 341                         gcdiv = gcd(cam_mod->custom.configs[i].
 342                                 frm_intrvl.interval.numerator,
 343                                 cam_mod->custom.configs[i].
 344                                         frm_intrvl.interval.denominator);
 345                         norm_interval.interval.numerator =
 346                                 cam_mod->custom.configs[i].
 347                                         frm_intrvl.interval.numerator / gcdiv;
 348                         norm_interval.interval.denominator =
 349                                 cam_mod->custom.configs[i].
 350                                 frm_intrvl.interval.denominator / gcdiv;
 351                         if ((frm_intrvl->interval.numerator !=
 352                                 norm_interval.interval.numerator) ||
 353                                 (frm_intrvl->interval.denominator !=
 354                                 norm_interval.interval.denominator))
 355                                 continue;
 356                 }
 357                 if (!IS_ERR_OR_NULL(fmt)) {
 358                         if ((cam_mod->custom.configs[i].frm_fmt.width !=
 359                                 fmt->width) ||
 360                                 (cam_mod->custom.configs[i].frm_fmt.height !=
 361                                 fmt->height) ||
 362                                 (cam_mod->custom.configs[i].frm_fmt.code !=
 363                                 fmt->code)) {
 364                                 continue;
 365                         }
 366                 }
 367                 pltfrm_camera_module_pr_debug(&cam_mod->sd,
 368                         "found matching config %s\n",
 369                         cam_mod->custom.configs[i].name);
 370                 return &cam_mod->custom.configs[i];
 371         }
 372         pltfrm_camera_module_pr_debug(&cam_mod->sd,
 373                 "no matching config found\n");
 374
 375         return ERR_PTR(-EINVAL);
 376 }
 377
 378 /* ======================================================================== */
 379
 380 static int aptina_camera_module_write_config(
 381         struct aptina_camera_module *cam_mod)
 382 {
 383         int ret = 0;
 384
 385         pltfrm_camera_module_pr_debug(&cam_mod->sd, "\n");
 386
 387         if (IS_ERR_OR_NULL(cam_mod->active_config)) {
 388                 pltfrm_camera_module_pr_err(&cam_mod->sd,
 389                         "no active sensor configuration");
 390                 ret = -EFAULT;
 391                 goto err;
 392         }
 393
 394         ret = aptina_write_reglist(&cam_mod->sd,
 395                 cam_mod->active_config->reg_table,
 396                 cam_mod->active_config->reg_table_num_entries);
 397         if (IS_ERR_VALUE(ret))
 398                 goto err;
 399         ret = pltfrm_camera_module_patch_config(&cam_mod->sd,
 400                 &cam_mod->frm_fmt,
 401                 &cam_mod->frm_intrvl);
 402         if (IS_ERR_VALUE(ret))
 403                 goto err;
 404
 405         return 0;
 406 err:
 407         pltfrm_camera_module_pr_err(&cam_mod->sd,
 408                 "failed with error %d\n", ret);
 409         return ret;
 410 }
 411
 412 static int aptina_camera_module_attach(
 413         struct aptina_camera_module *cam_mod)
 414 {
 415         int ret = 0;
 416         struct aptina_camera_module_custom_config *custom;
 417
 418         custom = &cam_mod->custom;
 419         pltfrm_camera_module_pr_debug(&cam_mod->sd, "\n");
 420         if (custom->check_camera_id) {
 421                 aptina_camera_module_s_power(&cam_mod->sd, 1);
 422                 ret = custom->check_camera_id(cam_mod);
 423                 aptina_camera_module_s_power(&cam_mod->sd, 0);
 424                 if (ret != 0)
 425                         goto err;
 426         }
 427
 428         return 0;
 429
 430 err:
 431         pltfrm_camera_module_pr_err(&cam_mod->sd,
 432                                 "failed with error %d\n", ret);
 433         aptina_camera_module_release(cam_mod);
 434         return ret;
 435 }
 436
 437 /* ======================================================================== */
 438
 439 int aptina_camera_module_try_fmt(struct v4l2_subdev *sd,
 440         struct v4l2_mbus_framefmt *fmt)
 441 {
 442         struct aptina_camera_module *cam_mod = to_aptina_camera_module(sd);
 443
 444         pltfrm_camera_module_pr_debug(&cam_mod->sd, "%dx%d, fmt code 0x%04x\n",
 445                 fmt->width, fmt->height, fmt->code);
 446
 447         if (IS_ERR_OR_NULL(
 448         aptina_camera_module_find_config(
 449         cam_mod, fmt, NULL))) {
 450                 pltfrm_camera_module_pr_debug(&cam_mod->sd,
 451                         "format not supported\n");
 452                 return -EINVAL;
 453         }
 454         pltfrm_camera_module_pr_debug(&cam_mod->sd, "format supported\n");
 455
 456         return 0;
 457 }
 458
 459 /* ======================================================================== */
 460
 461 int aptina_camera_module_s_fmt(struct v4l2_subdev *sd,
 462         struct v4l2_subdev_pad_config *cfg,
 463         struct v4l2_subdev_format *format)
 464 {
 465         struct aptina_camera_module *cam_mod =  to_aptina_camera_module(sd);
 466         struct v4l2_mbus_framefmt *fmt = &format->format;
 467         int ret = 0;
 468
 469         pltfrm_camera_module_pr_debug(&cam_mod->sd, "%dx%d, fmt code 0x%04x\n",
 470                 fmt->width, fmt->height, fmt->code);
 471
 472         if (IS_ERR_OR_NULL(
 473         aptina_camera_module_find_config(
 474         cam_mod, fmt, NULL))) {
 475                 pltfrm_camera_module_pr_err(&cam_mod->sd,
 476                         "format %dx%d, code 0x%04x, not supported\n",
 477                         fmt->width, fmt->height, fmt->code);
 478                 ret = -EINVAL;
 479                 goto err;
 480         }
 481         cam_mod->frm_fmt_valid = true;
 482         cam_mod->frm_fmt = *fmt;
 483         if (cam_mod->frm_intrvl_valid) {
 484                 aptina_camera_module_set_active_config(cam_mod,
 485                         aptina_camera_module_find_config(cam_mod,
 486                                 fmt, &cam_mod->frm_intrvl));
 487         }
 488         return 0;
 489 err:
 490         pltfrm_camera_module_pr_err(&cam_mod->sd,
 491                 "failed with error %d\n", ret);
 492         return ret;
 493 }
 494
 495 /* ======================================================================== */
 496
 497 int aptina_camera_module_g_fmt(struct v4l2_subdev *sd,
 498         struct v4l2_subdev_pad_config *cfg,
 499         struct v4l2_subdev_format *format)
 500 {
 501         struct aptina_camera_module *cam_mod =  to_aptina_camera_module(sd);
 502         struct v4l2_mbus_framefmt *fmt = &format->format;
 503
 504         pltfrm_camera_module_pr_debug(&cam_mod->sd, "\n");
 505
 506         if (cam_mod->active_config) {
 507                 fmt->code = cam_mod->active_config->frm_fmt.code;
 508                 fmt->width = cam_mod->active_config->frm_fmt.width;
 509                 fmt->height = cam_mod->active_config->frm_fmt.height;
 510                 return 0;
 511         }
 512
 513         pltfrm_camera_module_pr_debug(&cam_mod->sd, "no active config\n");
 514
 515         return -1;
 516 }
 517
 518 /* ======================================================================== */
 519
 520 int aptina_camera_module_s_frame_interval(
 521         struct v4l2_subdev *sd,
 522         struct v4l2_subdev_frame_interval *interval)
 523 {
 524         struct aptina_camera_module *cam_mod = to_aptina_camera_module(sd);
 525         unsigned long gcdiv;
 526         struct v4l2_subdev_frame_interval norm_interval;
 527         int ret = 0;
 528
 529         pltfrm_camera_module_pr_debug(&cam_mod->sd, "\n");
 530         if ((interval->interval.denominator == 0) ||
 531                 (interval->interval.numerator == 0)) {
 532                 pltfrm_camera_module_pr_err(&cam_mod->sd,
 533                         "invalid frame interval %d/%d\n",
 534                         interval->interval.numerator,
 535                         interval->interval.denominator);
 536                 ret = -EINVAL;
 537                 goto err;
 538         }
 539
 540         pltfrm_camera_module_pr_debug(&cam_mod->sd, "%d/%d (%dfps)\n",
 541                 interval->interval.numerator, interval->interval.denominator,
 542                 (interval->interval.denominator +
 543                 (interval->interval.numerator >> 1)) /
 544                 interval->interval.numerator);
 545
 546         /* normalize interval */
 547         gcdiv = gcd(interval->interval.numerator,
 548                 interval->interval.denominator);
 549         norm_interval.interval.numerator =
 550                 interval->interval.numerator / gcdiv;
 551         norm_interval.interval.denominator =
 552                 interval->interval.denominator / gcdiv;
 553
 554         if (IS_ERR_OR_NULL(aptina_camera_module_find_config(cam_mod,
 555                         NULL, &norm_interval))) {
 556                 pltfrm_camera_module_pr_err(&cam_mod->sd,
 557                         "frame interval %d/%d not supported\n",
 558                         interval->interval.numerator,
 559                         interval->interval.denominator);
 560                 ret = -EINVAL;
 561                 goto err;
 562         }
 563         cam_mod->frm_intrvl_valid = true;
 564         cam_mod->frm_intrvl = norm_interval;
 565         if (cam_mod->frm_fmt_valid) {
 566                 aptina_camera_module_set_active_config(cam_mod,
 567                         aptina_camera_module_find_config(cam_mod,
 568                                 &cam_mod->frm_fmt, interval));
 569         }
 570         return 0;
 571 err:
 572         pltfrm_camera_module_pr_err(&cam_mod->sd,
 573                 "failed with error %d\n", ret);
 574         return ret;
 575 }
 576
 577 /* ======================================================================== */
 578
 579 int aptina_camera_module_s_stream(struct v4l2_subdev *sd, int enable)
 580 {
 581         int ret = 0;
 582         struct aptina_camera_module *cam_mod =  to_aptina_camera_module(sd);
 583
 584         pltfrm_camera_module_pr_debug(&cam_mod->sd, "%d\n", enable);
 585
 586         if (enable) {
 587                 if (cam_mod->state == APTINA_CAMERA_MODULE_STREAMING)
 588                         return 0;
 589                 if (IS_ERR_OR_NULL(cam_mod->active_config)) {
 590                         pltfrm_camera_module_pr_err(&cam_mod->sd,
 591                                 "no active sensor configuration, cannot start streaming\n");
 592                         ret = -EFAULT;
 593                         goto err;
 594                 }
 595                 if (cam_mod->state != APTINA_CAMERA_MODULE_SW_STANDBY) {
 596                         pltfrm_camera_module_pr_err(&cam_mod->sd,
 597                                 "sensor is not powered on (in state %d), cannot start streaming\n",
 598                                 cam_mod->state);
 599                         ret = -EINVAL;
 600                         goto err;
 601                 }
 602
 603                 if (!IS_ERR_OR_NULL(cam_mod->custom.set_flip))
 604                         cam_mod->custom.set_flip(cam_mod);
 605
 606                 if (cam_mod->update_config)
 607                         ret = aptina_camera_module_write_config(cam_mod);
 608                         if (IS_ERR_VALUE(ret))
 609                                 goto err;
 610
 611                 ret = cam_mod->custom.start_streaming(cam_mod);
 612                 if (IS_ERR_VALUE(ret))
 613                         goto err;
 614                 cam_mod->update_config = false;
 615                 cam_mod->ctrl_updt = 0;
 616                 mdelay(cam_mod->custom.power_up_delays_ms[2]);
 617                 cam_mod->state = APTINA_CAMERA_MODULE_STREAMING;
 618         } else {
 619                 int pclk;
 620                 int wait_ms;
 621                 struct isp_supplemental_sensor_mode_data timings;
 622
 623                 if (cam_mod->state != APTINA_CAMERA_MODULE_STREAMING)
 624                         return 0;
 625                 ret = cam_mod->custom.stop_streaming(cam_mod);
 626                 if (IS_ERR_VALUE(ret))
 627                         goto err;
 628
 629                 cam_mod->state = APTINA_CAMERA_MODULE_SW_STANDBY;
 630
 631                 ret = aptina_camera_module_ioctl(sd, RK_VIDIOC_SENSOR_MODE_DATA,
 632                                              &timings);
 633                 if (IS_ERR_VALUE(ret))
 634                         goto err;
 635                 pclk = timings.vt_pix_clk_freq_hz / 1000;
 636                 if (!pclk)
 637                         goto err;
 638                 wait_ms = (timings.line_length_pck
 639                         * timings.frame_length_lines) / pclk;
 640                 /* wait for a frame period to make sure that there is
 641                  *no pending frame left.
 642                  */
 643                 msleep(wait_ms + 1);
 644         }
 645
 646         cam_mod->state_before_suspend = cam_mod->state;
 647
 648         return 0;
 649 err:
 650         pltfrm_camera_module_pr_err(&cam_mod->sd,
 651                 "failed with error %d\n", ret);
 652         return ret;
 653 }
 654
 655 /* ======================================================================== */
 656
 657 int aptina_camera_module_s_power(struct v4l2_subdev *sd, int on)
 658 {
 659         int ret = 0;
 660         struct aptina_camera_module *cam_mod =  to_aptina_camera_module(sd);
 661         struct v4l2_subdev *af_ctrl;
 662
 663         pltfrm_camera_module_pr_debug(&cam_mod->sd, "%d\n", on);
 664
 665         if (on) {
 666                 if (cam_mod->state == APTINA_CAMERA_MODULE_POWER_OFF) {
 667                         ret = pltfrm_camera_module_s_power(&cam_mod->sd, 1);
 668                         if (!IS_ERR_VALUE(ret)) {
 669                                 mdelay(cam_mod->custom.power_up_delays_ms[0]);
 670                                 cam_mod->state =
 671                                 APTINA_CAMERA_MODULE_HW_STANDBY;
 672                         }
 673                 }
 674                 if (cam_mod->state == APTINA_CAMERA_MODULE_HW_STANDBY) {
 675                         ret = pltfrm_camera_module_set_pin_state(&cam_mod->sd,
 676                             PLTFRM_CAMERA_MODULE_PIN_PWR,
 677                             PLTFRM_CAMERA_MODULE_PIN_STATE_ACTIVE);
 678                         msleep(20);
 679
 680                         ret = pltfrm_camera_module_set_pin_state(
 681                             &cam_mod->sd,
 682                             PLTFRM_CAMERA_MODULE_PIN_PD,
 683                             PLTFRM_CAMERA_MODULE_PIN_STATE_ACTIVE);
 684                         msleep(20);
 685
 686                         ret = pltfrm_camera_module_set_pin_state(
 687                             &cam_mod->sd,
 688                             PLTFRM_CAMERA_MODULE_PIN_RESET,
 689                             PLTFRM_CAMERA_MODULE_PIN_STATE_ACTIVE);
 690                         msleep(20);
 691
 692                         ret = pltfrm_camera_module_set_pin_state(
 693                             &cam_mod->sd,
 694                             PLTFRM_CAMERA_MODULE_PIN_RESET,
 695                             PLTFRM_CAMERA_MODULE_PIN_STATE_INACTIVE);
 696                         msleep(20);
 697
 698                         ret = pltfrm_camera_module_set_pin_state(
 699                             &cam_mod->sd,
 700                             PLTFRM_CAMERA_MODULE_PIN_RESET,
 701                             PLTFRM_CAMERA_MODULE_PIN_STATE_ACTIVE);
 702                         msleep(20);
 703
 704                         ret = pltfrm_camera_module_set_pin_state(
 705                             &cam_mod->sd,
 706                             PLTFRM_CAMERA_MODULE_PIN_PD,
 707                             PLTFRM_CAMERA_MODULE_PIN_STATE_INACTIVE);
 708                         msleep(20);
 709                         ret = pltfrm_camera_module_set_pin_state(
 710                             &cam_mod->sd,
 711                             PLTFRM_CAMERA_MODULE_PIN_PD,
 712                             PLTFRM_CAMERA_MODULE_PIN_STATE_ACTIVE);
 713                         msleep(20);
 714
 715                         if (!IS_ERR_VALUE(ret)) {
 716                                 mdelay(cam_mod->custom.power_up_delays_ms[1]);
 717                                 cam_mod->state =
 718                                 APTINA_CAMERA_MODULE_SW_STANDBY;
 719
 720                                 #if 1
 721                                 if (!IS_ERR_OR_NULL(
 722                                     cam_mod->custom.init_common) &&
 723                                     cam_mod->custom.init_common(
 724                                     cam_mod))
 725                                 usleep_range(1000, 1500);
 726                                 #endif
 727
 728                                 af_ctrl = pltfrm_camera_module_get_af_ctrl(sd);
 729                                 if (!IS_ERR_OR_NULL(af_ctrl)) {
 730                                         v4l2_subdev_call(af_ctrl,
 731                                                          core, init, 0);
 732                                 }
 733                         }
 734                 }
 735         } else {
 736                 if (cam_mod->state == APTINA_CAMERA_MODULE_STREAMING) {
 737                         ret = aptina_camera_module_s_stream(sd, 0);
 738                         if (!IS_ERR_VALUE(ret))
 739                                 cam_mod->state =
 740                                 APTINA_CAMERA_MODULE_SW_STANDBY;
 741                 }
 742                 if (cam_mod->state == APTINA_CAMERA_MODULE_SW_STANDBY) {
 743                         ret = pltfrm_camera_module_set_pin_state(
 744                             &cam_mod->sd,
 745                             PLTFRM_CAMERA_MODULE_PIN_PD,
 746                             PLTFRM_CAMERA_MODULE_PIN_STATE_INACTIVE);
 747
 748                         ret = pltfrm_camera_module_set_pin_state(
 749                             &cam_mod->sd,
 750                             PLTFRM_CAMERA_MODULE_PIN_RESET,
 751                             PLTFRM_CAMERA_MODULE_PIN_STATE_INACTIVE);
 752
 753                         ret = pltfrm_camera_module_set_pin_state(
 754                             &cam_mod->sd,
 755                             PLTFRM_CAMERA_MODULE_PIN_PWR,
 756                             PLTFRM_CAMERA_MODULE_PIN_STATE_INACTIVE);
 757
 758                         if (!IS_ERR_VALUE(ret))
 759                                 cam_mod->state =
 760                                 APTINA_CAMERA_MODULE_HW_STANDBY;
 761                 }
 762                 if (cam_mod->state == APTINA_CAMERA_MODULE_HW_STANDBY) {
 763                         ret = pltfrm_camera_module_s_power(&cam_mod->sd, 0);
 764                         if (!IS_ERR_VALUE(ret)) {
 765                                 cam_mod->state = APTINA_CAMERA_MODULE_POWER_OFF;
 766                                 aptina_camera_module_reset(cam_mod);
 767                         }
 768                 }
 769         }
 770
 771         cam_mod->state_before_suspend = cam_mod->state;
 772
 773         if (IS_ERR_VALUE(ret)) {
 774                 pltfrm_camera_module_pr_err(&cam_mod->sd,
 775                         "%s failed, camera left in state %d\n",
 776                         on ? "on" : "off", cam_mod->state);
 777                 goto err;
 778         } else
 779                 pltfrm_camera_module_pr_debug(&cam_mod->sd,
 780                         "camera powered %s\n", on ? "on" : "off");
 781
 782         return 0;
 783 err:
 784         pltfrm_camera_module_pr_err(&cam_mod->sd,
 785                 "failed with error %d\n", ret);
 786         return ret;
 787 }
 788
 789 /* ======================================================================== */
 790
 791 int aptina_camera_module_g_ctrl(struct v4l2_subdev *sd,
 792         struct v4l2_control *ctrl)
 793 {
 794         struct aptina_camera_module *cam_mod = to_aptina_camera_module(sd);
 795         int ret;
 796
 797         pltfrm_camera_module_pr_debug(&cam_mod->sd, " id 0x%x\n", ctrl->id);
 798
 799         if (ctrl->id == V4L2_CID_FLASH_LED_MODE) {
 800                 ctrl->value = cam_mod->exp_config.flash_mode;
 801                 pltfrm_camera_module_pr_debug(&cam_mod->sd,
 802                         "V4L2_CID_FLASH_LED_MODE %d\n",
 803                         ctrl->value);
 804                 return 0;
 805         }
 806
 807         if (IS_ERR_OR_NULL(cam_mod->active_config)) {
 808                 pltfrm_camera_module_pr_err(&cam_mod->sd,
 809                         "no active configuration\n");
 810                 return -EFAULT;
 811         }
 812
 813         if (ctrl->id == RK_V4L2_CID_VBLANKING) {
 814                 ctrl->value = cam_mod->active_config->v_blanking_time_us;
 815                 pltfrm_camera_module_pr_debug(&cam_mod->sd,
 816                         "RK_V4L2_CID_VBLANKING %d\n",
 817                         ctrl->value);
 818                 return 0;
 819         }
 820
 821 /*
 822  *if (ctrl->id == RK_V4L2_CID_IGNORE_MEASUREMENT_CHECK) {
 823  *      ctrl->value = cam_mod->active_config->ignore_measurement_check;
 824  *              pltfrm_camera_module_pr_debug(
 825  *                      &cam_mod->sd,
 826  *                      "CIF_ISP20_CID_IGNORE_MEASUREMENT_CHECK %d\n",
 827  *                      ctrl->value);
 828  *              return 0;
 829  *      }
 830  */
 831
 832         if ((cam_mod->state != APTINA_CAMERA_MODULE_SW_STANDBY) &&
 833                 (cam_mod->state != APTINA_CAMERA_MODULE_STREAMING)) {
 834                 pltfrm_camera_module_pr_err(&cam_mod->sd,
 835                         "cannot get controls when camera is off\n");
 836                 return -EFAULT;
 837         }
 838
 839         if (ctrl->id == V4L2_CID_FOCUS_ABSOLUTE) {
 840                 struct v4l2_subdev *af_ctrl;
 841
 842                 af_ctrl =
 843                         pltfrm_camera_module_get_af_ctrl(sd);
 844                 if (!IS_ERR_OR_NULL(af_ctrl)) {
 845                         ret = v4l2_subdev_call(af_ctrl, core, g_ctrl, ctrl);
 846                         return ret;
 847                 }
 848         }
 849
 850         if (!IS_ERR_OR_NULL(cam_mod->custom.g_ctrl)) {
 851                 ret = cam_mod->custom.g_ctrl(cam_mod, ctrl->id);
 852                 if (IS_ERR_VALUE(ret))
 853                         return ret;
 854         }
 855
 856         switch (ctrl->id) {
 857         case V4L2_CID_GAIN:
 858                 ctrl->value = cam_mod->exp_config.gain;
 859                 pltfrm_camera_module_pr_debug(&cam_mod->sd,
 860                              "V4L2_CID_GAIN %d\n",
 861                              ctrl->value);
 862                 break;
 863         case V4L2_CID_EXPOSURE:
 864                 ctrl->value = cam_mod->exp_config.exp_time;
 865                 pltfrm_camera_module_pr_debug(&cam_mod->sd,
 866                              "V4L2_CID_EXPOSURE %d\n",
 867                              ctrl->value);
 868                 break;
 869         case V4L2_CID_WHITE_BALANCE_TEMPERATURE:
 870                 ctrl->value = cam_mod->wb_config.temperature;
 871                 pltfrm_camera_module_pr_debug(&cam_mod->sd,
 872                         "V4L2_CID_WHITE_BALANCE_TEMPERATURE %d\n",
 873                         ctrl->value);
 874                 break;
 875         case V4L2_CID_AUTOGAIN:
 876                 ctrl->value = cam_mod->exp_config.auto_gain;
 877                 pltfrm_camera_module_pr_debug(&cam_mod->sd,
 878                         "V4L2_CID_AUTOGAIN %d\n",
 879                         ctrl->value);
 880                 break;
 881         case V4L2_CID_EXPOSURE_AUTO:
 882                 ctrl->value = cam_mod->exp_config.auto_exp;
 883                 pltfrm_camera_module_pr_debug(&cam_mod->sd,
 884                         "V4L2_CID_EXPOSURE_AUTO %d\n",
 885                         ctrl->value);
 886                 break;
 887         case V4L2_CID_AUTO_WHITE_BALANCE:
 888                 ctrl->value = cam_mod->wb_config.auto_wb;
 889                 pltfrm_camera_module_pr_debug(&cam_mod->sd,
 890                         "V4L2_CID_AUTO_WHITE_BALANCE %d\n",
 891                         ctrl->value);
 892                 break;
 893         case V4L2_CID_FOCUS_ABSOLUTE:
 894                 ctrl->value = cam_mod->af_config.abs_pos;
 895                 pltfrm_camera_module_pr_debug(&cam_mod->sd,
 896                         "V4L2_CID_FOCUS_ABSOLUTE %d\n",
 897                         ctrl->value);
 898                 break;
 899         case V4L2_CID_HFLIP:
 900         case V4L2_CID_VFLIP:
 901                 /* TBD */
 902                 /* fallthrough */
 903         default:
 904                 pltfrm_camera_module_pr_debug(&cam_mod->sd,
 905                         "failed, unknown ctrl %d\n", ctrl->id);
 906                 return -EINVAL;
 907         }
 908
 909         return 0;
 910 }
 911
 912 /* ======================================================================== */
 913
 914 static int flash_light_ctrl(
 915                 struct v4l2_subdev *sd,
 916                 struct aptina_camera_module *cam_mod,
 917                 int value)
 918 {
 919         return 0;
 920 }
 921
 922 /* ======================================================================== */
 923
 924 int aptina_camera_module_s_ext_ctrls(
 925         struct v4l2_subdev *sd,
 926         struct v4l2_ext_controls *ctrls)
 927 {
 928         int i;
 929         int ctrl_cnt = 0;
 930         struct aptina_camera_module *cam_mod =  to_aptina_camera_module(sd);
 931         int ret = 0;
 932
 933         pltfrm_camera_module_pr_debug(&cam_mod->sd, "\n");
 934
 935         if (ctrls->count == 0)
 936                 return -EINVAL;
 937
 938         for (i = 0; i < ctrls->count; i++) {
 939                 struct v4l2_ext_control *ctrl;
 940                 u32 ctrl_updt = 0;
 941
 942                 ctrl = &ctrls->controls[i];
 943
 944                 switch (ctrl->id) {
 945                 case V4L2_CID_GAIN:
 946                         ctrl_updt = APTINA_CAMERA_MODULE_CTRL_UPDT_GAIN;
 947                         cam_mod->exp_config.gain = ctrl->value;
 948                         pltfrm_camera_module_pr_debug(&cam_mod->sd,
 949                         "V4L2_CID_GAIN %d\n",
 950                         ctrl->value);
 951                         break;
 952                 case RK_V4L2_CID_GAIN_PERCENT:
 953                         ctrl_updt = APTINA_CAMERA_MODULE_CTRL_UPDT_GAIN;
 954                         cam_mod->exp_config.gain_percent = ctrl->value;
 955                         break;
 956                 case V4L2_CID_FLASH_LED_MODE:
 957                         ret = flash_light_ctrl(sd, cam_mod, ctrl->value);
 958                         if (ret == 0) {
 959                                 cam_mod->exp_config.flash_mode = ctrl->value;
 960                                 pltfrm_camera_module_pr_debug(
 961                                         &cam_mod->sd,
 962                                         "V4L2_CID_FLASH_LED_MODE %d\n",
 963                                         ctrl->value);
 964                         }
 965                         break;
 966                 case V4L2_CID_EXPOSURE:
 967                         ctrl_updt = APTINA_CAMERA_MODULE_CTRL_UPDT_EXP_TIME;
 968                         cam_mod->exp_config.exp_time = ctrl->value;
 969                         pltfrm_camera_module_pr_debug(&cam_mod->sd,
 970                         "V4L2_CID_EXPOSURE %d\n",
 971                         ctrl->value);
 972                         break;
 973                 case V4L2_CID_WHITE_BALANCE_TEMPERATURE:
 974                         ctrl_updt =
 975                         APTINA_CAMERA_MODULE_CTRL_UPDT_WB_TEMPERATURE;
 976                         cam_mod->wb_config.temperature = ctrl->value;
 977                         pltfrm_camera_module_pr_debug(&cam_mod->sd,
 978                         "V4L2_CID_WHITE_BALANCE_TEMPERATURE %d\n",
 979                         ctrl->value);
 980                         break;
 981                 case V4L2_CID_AUTOGAIN:
 982                         ctrl_updt = APTINA_CAMERA_MODULE_CTRL_UPDT_AUTO_GAIN;
 983                         cam_mod->exp_config.auto_gain = ctrl->value;
 984                         pltfrm_camera_module_pr_debug(&cam_mod->sd,
 985                         "V4L2_CID_AUTOGAIN %d\n",
 986                         ctrl->value);
 987                         break;
 988                 case V4L2_CID_EXPOSURE_AUTO:
 989                         ctrl_updt = APTINA_CAMERA_MODULE_CTRL_UPDT_AUTO_EXP;
 990                         cam_mod->exp_config.auto_exp = ctrl->value;
 991                         pltfrm_camera_module_pr_debug(&cam_mod->sd,
 992                         "V4L2_CID_EXPOSURE_AUTO %d\n",
 993                         ctrl->value);
 994                         break;
 995                 case V4L2_CID_AUTO_WHITE_BALANCE:
 996                         ctrl_updt = APTINA_CAMERA_MODULE_CTRL_UPDT_AUTO_WB;
 997                         cam_mod->wb_config.auto_wb = ctrl->value;
 998                         pltfrm_camera_module_pr_debug(&cam_mod->sd,
 999                         "V4L2_CID_AUTO_WHITE_BALANCE %d\n",
1000                         ctrl->value);
1001                         break;
1002                 case RK_V4L2_CID_AUTO_FPS:
1003                         cam_mod->auto_adjust_fps = ctrl->value;
1004                         pltfrm_camera_module_pr_debug(&cam_mod->sd,
1005                         "RK_V4L2_CID_AUTO_FPS %d\n",
1006                         ctrl->value);
1007                         break;
1008                 case V4L2_CID_FOCUS_ABSOLUTE:
1009                         {
1010                                 struct v4l2_subdev *af_ctrl;
1011
1012                                 af_ctrl = pltfrm_camera_module_get_af_ctrl(sd);
1013                                 if (!IS_ERR_OR_NULL(af_ctrl)) {
1014                                         struct v4l2_control single_ctrl;
1015
1016                                         single_ctrl.id =
1017                                                 V4L2_CID_FOCUS_ABSOLUTE;
1018                                         single_ctrl.value = ctrl->value;
1019                                         ret = v4l2_subdev_call(af_ctrl,
1020                                                 core, s_ctrl, &single_ctrl);
1021                                         return ret;
1022                                 }
1023                         }
1024                         ctrl_updt =
1025                                 APTINA_CAMERA_MODULE_CTRL_UPDT_FOCUS_ABSOLUTE;
1026                         cam_mod->af_config.abs_pos = ctrl->value;
1027                         pltfrm_camera_module_pr_debug(&cam_mod->sd,
1028                         "V4L2_CID_FOCUS_ABSOLUTE %d\n",
1029                         ctrl->value);
1030                         break;
1031                 case V4L2_CID_HFLIP:
1032                         if (ctrl->value)
1033                                 cam_mod->hflip = true;
1034                         else
1035                                 cam_mod->hflip = false;
1036                         break;
1037                 case V4L2_CID_VFLIP:
1038                         if (ctrl->value)
1039                                 cam_mod->vflip = true;
1040                         else
1041                                 cam_mod->vflip = false;
1042                         break;
1043                 default:
1044                         pltfrm_camera_module_pr_warn(&cam_mod->sd,
1045                         "ignoring unknown ctrl 0x%x\n", ctrl->id);
1046                         break;
1047                 }
1048
1049                 if (cam_mod->state != APTINA_CAMERA_MODULE_SW_STANDBY &&
1050                 cam_mod->state != APTINA_CAMERA_MODULE_STREAMING)
1051                         cam_mod->ctrl_updt |= ctrl_updt;
1052                 else if (ctrl_updt)
1053                         ctrl_cnt++;
1054         }
1055
1056         /* if camera module is already streaming, write through */
1057         if (ctrl_cnt &&
1058                 (cam_mod->state == APTINA_CAMERA_MODULE_STREAMING ||
1059                 cam_mod->state == APTINA_CAMERA_MODULE_SW_STANDBY)) {
1060                 struct aptina_camera_module_ext_ctrls aptina_ctrls;
1061
1062                 aptina_ctrls.ctrls =
1063                 (struct aptina_camera_module_ext_ctrl *)
1064                 kmalloc(ctrl_cnt * sizeof(struct aptina_camera_module_ext_ctrl),
1065                         GFP_KERNEL);
1066
1067                 if (aptina_ctrls.ctrls) {
1068                         for (i = 0; i < ctrl_cnt; i++) {
1069                                 aptina_ctrls.ctrls[i].id =
1070                                         ctrls->controls[i].id;
1071                                 aptina_ctrls.ctrls[i].value =
1072                                         ctrls->controls[i].value;
1073                         }
1074
1075                         aptina_ctrls.count = ctrl_cnt;
1076
1077                         ret = cam_mod->custom.s_ext_ctrls(cam_mod,
1078                                                 &aptina_ctrls);
1079
1080                         kfree(aptina_ctrls.ctrls);
1081                 } else {
1082                         ret = -ENOMEM;
1083                 }
1084
1085                 if (IS_ERR_VALUE(ret))
1086                         pltfrm_camera_module_pr_debug(&cam_mod->sd,
1087                                 "failed with error %d\n", ret);
1088         }
1089
1090         return ret;
1091 }
1092
1093 /* ======================================================================== */
1094
1095 int aptina_camera_module_s_ctrl(
1096         struct v4l2_subdev *sd,
1097         struct v4l2_control *ctrl)
1098 {
1099         struct aptina_camera_module *cam_mod =  to_aptina_camera_module(sd);
1100         struct v4l2_ext_control ext_ctrl[1];
1101         struct v4l2_ext_controls ext_ctrls;
1102
1103         pltfrm_camera_module_pr_debug(&cam_mod->sd,
1104                 "0x%x 0x%x\n", ctrl->id, ctrl->value);
1105
1106         ext_ctrl[0].id = ctrl->id;
1107         ext_ctrl[0].value = ctrl->value;
1108
1109         ext_ctrls.count = 1;
1110         ext_ctrls.controls = ext_ctrl;
1111
1112         return aptina_camera_module_s_ext_ctrls(sd, &ext_ctrls);
1113 }
1114
1115 /* ======================================================================== */
1116
1117 long aptina_camera_module_ioctl(struct v4l2_subdev *sd,
1118         unsigned int cmd,
1119         void *arg)
1120 {
1121         struct aptina_camera_module *cam_mod =  to_aptina_camera_module(sd);
1122         int ret;
1123
1124         pltfrm_camera_module_pr_debug(&cam_mod->sd, "\n");
1125
1126         if (cmd == RK_VIDIOC_SENSOR_MODE_DATA) {
1127                 struct aptina_camera_module_timings aptina_timings;
1128                 struct isp_supplemental_sensor_mode_data *timings =
1129                 (struct isp_supplemental_sensor_mode_data *)arg;
1130
1131                 if (cam_mod->custom.g_timings)
1132                         ret = cam_mod->custom.g_timings(cam_mod,
1133                                                 &aptina_timings);
1134                 else
1135                         ret = -EPERM;
1136
1137                 if (IS_ERR_VALUE(ret)) {
1138                         pltfrm_camera_module_pr_err(&cam_mod->sd,
1139                         "failed with error %d\n", ret);
1140                         return ret;
1141                 }
1142
1143                 timings->sensor_output_width =
1144                         aptina_timings.sensor_output_width;
1145                 timings->sensor_output_height =
1146                         aptina_timings.sensor_output_height;
1147                 timings->crop_horizontal_start =
1148                         aptina_timings.crop_horizontal_start;
1149                 timings->crop_vertical_start =
1150                         aptina_timings.crop_vertical_start;
1151                 timings->crop_horizontal_end =
1152                         aptina_timings.crop_horizontal_end;
1153                 timings->crop_vertical_end =
1154                         aptina_timings.crop_vertical_end;
1155                 timings->line_length_pck =
1156                         aptina_timings.line_length_pck;
1157                 timings->frame_length_lines =
1158                         aptina_timings.frame_length_lines;
1159                 timings->vt_pix_clk_freq_hz =
1160                         aptina_timings.vt_pix_clk_freq_hz;
1161                 timings->binning_factor_x =
1162                         aptina_timings.binning_factor_x;
1163                 timings->binning_factor_y =
1164                         aptina_timings.binning_factor_y;
1165                 timings->coarse_integration_time_max_margin =
1166                         aptina_timings.coarse_integration_time_max_margin;
1167                 timings->coarse_integration_time_min =
1168                         aptina_timings.coarse_integration_time_min;
1169                 timings->fine_integration_time_max_margin =
1170                         aptina_timings.fine_integration_time_max_margin;
1171                 timings->fine_integration_time_min =
1172                         aptina_timings.fine_integration_time_min;
1173
1174                 if (cam_mod->custom.g_exposure_valid_frame)
1175                         timings->exposure_valid_frame =
1176                                 cam_mod->custom.g_exposure_valid_frame(cam_mod);
1177
1178                 /*
1179                  *timings->exp_time = cam_mod->exp_config.exp_time;
1180                  *timings->gain = cam_mod->exp_config.gain;
1181                  */
1182
1183                 return ret;
1184         } else if (cmd == PLTFRM_CIFCAM_G_ITF_CFG) {
1185                 struct pltfrm_cam_itf *itf_cfg = (struct pltfrm_cam_itf *)arg;
1186                 struct aptina_camera_module_config *config;
1187
1188                 if (cam_mod->custom.num_configs <= 0) {
1189                         pltfrm_camera_module_pr_err(&cam_mod->sd,
1190                                 "cam_mod->custom.num_configs is NULL, Get interface config failed!\n");
1191                         return -EINVAL;
1192                 }
1193
1194                 if (IS_ERR_OR_NULL(cam_mod->active_config))
1195                         config = &cam_mod->custom.configs[0];
1196                 else
1197                         config = cam_mod->active_config;
1198
1199                 *itf_cfg = config->itf_cfg;
1200                 pltfrm_camera_module_ioctl(sd, PLTFRM_CIFCAM_G_ITF_CFG, arg);
1201                 return 0;
1202         } else if (cmd == PLTFRM_CIFCAM_ATTACH) {
1203                 aptina_camera_module_init(cam_mod, &cam_mod->custom);
1204                 pltfrm_camera_module_ioctl(sd, cmd, arg);
1205                 return aptina_camera_module_attach(cam_mod);
1206         }
1207
1208         ret = pltfrm_camera_module_ioctl(sd, cmd, arg);
1209         return ret;
1210 }
1211
1212 /* Reconfigure bayer order according to xml's definition */
1213 static int aptina_camera_module_s_flip(struct aptina_camera_module *cam_mod)
1214 {
1215         int i, mode = 0;
1216         static int flip_mode;
1217
1218         if (!cam_mod->custom.configs->bayer_order_alter_enble) {
1219                 aptina_camera_module_pr_info(cam_mod,
1220                                          "not need to change bayer order!");
1221                 return 0;
1222         }
1223
1224         mode = pltfrm_camera_module_get_flip_mirror(&cam_mod->sd);
1225
1226         aptina_camera_module_pr_debug(cam_mod,
1227                         "%s(%d): flip_mode is %d, mode is %d.\n",
1228                         __func__,
1229                         __LINE__,
1230                         flip_mode,
1231                         mode);
1232
1233         if (mode != flip_mode) {
1234                 /* Reconfigure the compatible bayer order */
1235                 switch (mode) {
1236                 case 0:
1237                         /* Change all the bayter order of registered configs */
1238                         for (i = 0; i < cam_mod->custom.num_configs; i++)
1239                                 cam_mod->custom.configs[i].frm_fmt.code =
1240                                         MEDIA_BUS_FMT_SGRBG10_1X10;
1241                         break;
1242
1243                 case 1:
1244                         for (i = 0; i < cam_mod->custom.num_configs; i++)
1245                                 cam_mod->custom.configs[i].frm_fmt.code =
1246                                         MEDIA_BUS_FMT_SRGGB10_1X10;
1247                         break;
1248
1249                 case 2:
1250                         for (i = 0; i < cam_mod->custom.num_configs; i++)
1251                                 cam_mod->custom.configs[i].frm_fmt.code =
1252                                         MEDIA_BUS_FMT_SBGGR10_1X10;
1253                         break;
1254
1255                 case 3:
1256                         for (i = 0; i < cam_mod->custom.num_configs; i++)
1257                                 cam_mod->custom.configs[i].frm_fmt.code =
1258                                         MEDIA_BUS_FMT_SGBRG10_1X10;
1259                         break;
1260
1261                 default:
1262                         break;
1263                 };
1264         }
1265
1266         flip_mode = mode;
1267
1268         return 0;
1269 }
1270
1271 int aptina_camera_module_get_flip_mirror(
1272         struct aptina_camera_module *cam_mod)
1273 {
1274         return pltfrm_camera_module_get_flip_mirror(&cam_mod->sd);
1275 }
1276
1277 int aptina_camera_module_enum_frameintervals(
1278         struct v4l2_subdev *sd,
1279         struct v4l2_subdev_pad_config *cfg,
1280         struct v4l2_subdev_frame_interval_enum *file)
1281 {
1282         struct aptina_camera_module *cam_mod =  to_aptina_camera_module(sd);
1283
1284         pltfrm_camera_module_pr_debug(&cam_mod->sd, "%d\n", file->index);
1285
1286         if (file->index >= cam_mod->custom.num_configs)
1287                 return -EINVAL;
1288
1289         aptina_camera_module_s_flip(cam_mod);
1290         file->code =
1291                 cam_mod->custom.configs[file->index].frm_fmt.code;
1292         file->width = cam_mod->custom.configs[file->index].frm_fmt.width;
1293         file->height = cam_mod->custom.configs[file->index].frm_fmt.height;
1294         file->interval.numerator = cam_mod->custom.
1295                 configs[file->index].frm_intrvl.interval.numerator;
1296         file->interval.denominator = cam_mod->custom.
1297                 configs[file->index].frm_intrvl.interval.denominator;
1298         return 0;
1299 }
1300
1301 /* ======================================================================== */
1302
1303 int aptina_camera_module_write_reglist(
1304         struct aptina_camera_module *cam_mod,
1305         const struct aptina_camera_module_reg reglist[],
1306         int len)
1307 {
1308         return aptina_write_reglist(&cam_mod->sd, reglist, len);
1309 }
1310
1311 /* ======================================================================== */
1312
1313 int aptina_camera_module_write_reg(
1314         struct aptina_camera_module *cam_mod,
1315         u16 reg,
1316         u16 val)
1317 {
1318         return aptina_write_i2c_reg(&cam_mod->sd, reg, val);
1319 }
1320
1321 /* ======================================================================== */
1322
1323 int aptina_camera_module_read_reg(
1324         struct aptina_camera_module *cam_mod,
1325         u16 data_length,
1326         u16 reg,
1327         u32 *val)
1328 {
1329         return aptina_read_i2c_reg(&cam_mod->sd,
1330                 data_length, reg, val);
1331 }
1332
1333 /* ======================================================================== */
1334
1335 int aptina_camera_module_read_reg_table(
1336         struct aptina_camera_module *cam_mod,
1337         u16 reg,
1338         u32 *val)
1339 {
1340         int i;
1341
1342         if (cam_mod->state == APTINA_CAMERA_MODULE_STREAMING)
1343                 return aptina_read_i2c_reg(&cam_mod->sd,
1344                         1, reg, val);
1345
1346         if (!IS_ERR_OR_NULL(cam_mod->active_config)) {
1347                 for (
1348                         i = cam_mod->active_config->reg_table_num_entries - 1;
1349                         i > 0;
1350                         i--) {
1351                         if (cam_mod->active_config->reg_table[i].reg == reg) {
1352                                 *val = cam_mod->active_config->reg_table[i].val;
1353                                 return 0;
1354                         }
1355                 }
1356         }
1357
1358         if (cam_mod->state == APTINA_CAMERA_MODULE_SW_STANDBY)
1359                 return aptina_read_i2c_reg(&cam_mod->sd,
1360                         1, reg, val);
1361
1362         return -EFAULT;
1363 }
1364
1365 /* ======================================================================== */
1366
1367 int aptina_camera_module_init(struct aptina_camera_module *cam_mod,
1368         struct aptina_camera_module_custom_config *custom)
1369 {
1370         int ret = 0;
1371
1372         pltfrm_camera_module_pr_debug(&cam_mod->sd, "\n");
1373
1374         aptina_camera_module_reset(cam_mod);
1375
1376         if (IS_ERR_OR_NULL(custom->start_streaming) ||
1377                 IS_ERR_OR_NULL(custom->stop_streaming) ||
1378                 IS_ERR_OR_NULL(custom->s_ctrl) ||
1379                 IS_ERR_OR_NULL(custom->g_ctrl)) {
1380                 pltfrm_camera_module_pr_err(&cam_mod->sd,
1381                         "mandatory callback function is missing\n");
1382                 ret = -EINVAL;
1383                 goto err;
1384         }
1385
1386         ret = pltfrm_camera_module_init(&cam_mod->sd, &cam_mod->pltfm_data);
1387         if (IS_ERR_VALUE(ret))
1388                 goto err;
1389
1390         ret = pltfrm_camera_module_set_pin_state(&cam_mod->sd,
1391                                 PLTFRM_CAMERA_MODULE_PIN_PD,
1392                                 PLTFRM_CAMERA_MODULE_PIN_STATE_INACTIVE);
1393         ret = pltfrm_camera_module_set_pin_state(&cam_mod->sd,
1394                                 PLTFRM_CAMERA_MODULE_PIN_RESET,
1395                                 PLTFRM_CAMERA_MODULE_PIN_STATE_ACTIVE);
1396         if (IS_ERR_VALUE(ret)) {
1397                 aptina_camera_module_release(cam_mod);
1398                 goto err;
1399         }
1400 /*
1401  *if (custom->check_camera_id) {
1402  *      aptina_camera_module_s_power(&cam_mod->sd, 1);
1403  *      ret = (custom->check_camera_id)(cam_mod);
1404  *      aptina_camera_module_s_power(&cam_mod->sd, 0);
1405  *}
1406  *
1407  *if (IS_ERR_VALUE(ret)) {
1408  *      aptina_camera_module_release(cam_mod);
1409  *      goto err;
1410  *}
1411  */
1412
1413         return 0;
1414 err:
1415         pltfrm_camera_module_pr_err(&cam_mod->sd,
1416                 "failed with error %d\n", ret);
1417         return ret;
1418 }
1419
1420 void aptina_camera_module_release(struct aptina_camera_module *cam_mod)
1421 {
1422         pltfrm_camera_module_pr_debug(&cam_mod->sd, "\n");
1423
1424         cam_mod->custom.configs = NULL;
1425
1426         pltfrm_camera_module_release(&cam_mod->sd);
1427         v4l2_device_unregister_subdev(&cam_mod->sd);
1428 }