drivers/staging/media/atomisp/i2c/atomisp-ov2722.c

   1 // SPDX-License-Identifier: GPL-2.0
   2 /*
   3  * Support for OmniVision OV2722 1080p HD camera sensor.
   4  *
   5  * Copyright (c) 2013 Intel Corporation. All Rights Reserved.
   6  *
   7  * This program is free software; you can redistribute it and/or
   8  * modify it under the terms of the GNU General Public License version
   9  * 2 as published by the Free Software Foundation.
  10  *
  11  * This program is distributed in the hope that it will be useful,
  12  * but WITHOUT ANY WARRANTY; without even the implied warranty of
  13  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
  14  * GNU General Public License for more details.
  15  *
  16  */
  17
  18 #include <linux/module.h>
  19 #include <linux/types.h>
  20 #include <linux/kernel.h>
  21 #include <linux/mm.h>
  22 #include <linux/string.h>
  23 #include <linux/errno.h>
  24 #include <linux/init.h>
  25 #include <linux/kmod.h>
  26 #include <linux/device.h>
  27 #include <linux/delay.h>
  28 #include <linux/slab.h>
  29 #include <linux/i2c.h>
  30 #include <linux/moduleparam.h>
  31 #include <media/v4l2-device.h>
  32 #include "../include/linux/atomisp_gmin_platform.h"
  33 #include <linux/acpi.h>
  34 #include <linux/io.h>
  35
  36 #include "ov2722.h"
  37
  38 /* i2c read/write stuff */
  39 static int ov2722_read_reg(struct i2c_client *client,
  40                            u16 data_length, u16 reg, u16 *val)
  41 {
  42         int err;
  43         struct i2c_msg msg[2];
  44         unsigned char data[6];
  45
  46         if (!client->adapter) {
  47                 dev_err(&client->dev, "%s error, no client->adapter\n",
  48                         __func__);
  49                 return -ENODEV;
  50         }
  51
  52         if (data_length != OV2722_8BIT && data_length != OV2722_16BIT &&
  53             data_length != OV2722_32BIT) {
  54                 dev_err(&client->dev, "%s error, invalid data length\n",
  55                         __func__);
  56                 return -EINVAL;
  57         }
  58
  59         memset(msg, 0, sizeof(msg));
  60
  61         msg[0].addr = client->addr;
  62         msg[0].flags = 0;
  63         msg[0].len = I2C_MSG_LENGTH;
  64         msg[0].buf = data;
  65
  66         /* high byte goes out first */
  67         data[0] = (u8)(reg >> 8);
  68         data[1] = (u8)(reg & 0xff);
  69
  70         msg[1].addr = client->addr;
  71         msg[1].len = data_length;
  72         msg[1].flags = I2C_M_RD;
  73         msg[1].buf = data;
  74
  75         err = i2c_transfer(client->adapter, msg, 2);
  76         if (err != 2) {
  77                 if (err >= 0)
  78                         err = -EIO;
  79                 dev_err(&client->dev,
  80                         "read from offset 0x%x error %d", reg, err);
  81                 return err;
  82         }
  83
  84         *val = 0;
  85         /* high byte comes first */
  86         if (data_length == OV2722_8BIT)
  87                 *val = (u8)data[0];
  88         else if (data_length == OV2722_16BIT)
  89                 *val = be16_to_cpu(*(__be16 *)&data[0]);
  90         else
  91                 *val = be32_to_cpu(*(__be32 *)&data[0]);
  92
  93         return 0;
  94 }
  95
  96 static int ov2722_i2c_write(struct i2c_client *client, u16 len, u8 *data)
  97 {
  98         struct i2c_msg msg;
  99         const int num_msg = 1;
 100         int ret;
 101
 102         msg.addr = client->addr;
 103         msg.flags = 0;
 104         msg.len = len;
 105         msg.buf = data;
 106         ret = i2c_transfer(client->adapter, &msg, 1);
 107
 108         return ret == num_msg ? 0 : -EIO;
 109 }
 110
 111 static int ov2722_write_reg(struct i2c_client *client, u16 data_length,
 112                             u16 reg, u16 val)
 113 {
 114         int ret;
 115         unsigned char data[4] = {0};
 116         __be16 *wreg = (__be16 *)data;
 117         const u16 len = data_length + sizeof(u16); /* 16-bit address + data */
 118
 119         if (data_length != OV2722_8BIT && data_length != OV2722_16BIT) {
 120                 dev_err(&client->dev,
 121                         "%s error, invalid data_length\n", __func__);
 122                 return -EINVAL;
 123         }
 124
 125         /* high byte goes out first */
 126         *wreg = cpu_to_be16(reg);
 127
 128         if (data_length == OV2722_8BIT) {
 129                 data[2] = (u8)(val);
 130         } else {
 131                 /* OV2722_16BIT */
 132                 __be16 *wdata = (__be16 *)&data[2];
 133
 134                 *wdata = cpu_to_be16(val);
 135         }
 136
 137         ret = ov2722_i2c_write(client, len, data);
 138         if (ret)
 139                 dev_err(&client->dev,
 140                         "write error: wrote 0x%x to offset 0x%x error %d",
 141                         val, reg, ret);
 142
 143         return ret;
 144 }
 145
 146 /*
 147  * ov2722_write_reg_array - Initializes a list of OV2722 registers
 148  * @client: i2c driver client structure
 149  * @reglist: list of registers to be written
 150  *
 151  * This function initializes a list of registers. When consecutive addresses
 152  * are found in a row on the list, this function creates a buffer and sends
 153  * consecutive data in a single i2c_transfer().
 154  *
 155  * __ov2722_flush_reg_array, __ov2722_buf_reg_array() and
 156  * __ov2722_write_reg_is_consecutive() are internal functions to
 157  * ov2722_write_reg_array_fast() and should be not used anywhere else.
 158  *
 159  */
 160
 161 static int __ov2722_flush_reg_array(struct i2c_client *client,
 162                                     struct ov2722_write_ctrl *ctrl)
 163 {
 164         u16 size;
 165         __be16 *data16 = (void *)&ctrl->buffer.addr;
 166
 167         if (ctrl->index == 0)
 168                 return 0;
 169
 170         size = sizeof(u16) + ctrl->index; /* 16-bit address + data */
 171         *data16 = cpu_to_be16(ctrl->buffer.addr);
 172         ctrl->index = 0;
 173
 174         return ov2722_i2c_write(client, size, (u8 *)&ctrl->buffer);
 175 }
 176
 177 static int __ov2722_buf_reg_array(struct i2c_client *client,
 178                                   struct ov2722_write_ctrl *ctrl,
 179                                   const struct ov2722_reg *next)
 180 {
 181         int size;
 182         __be16 *data16;
 183
 184         switch (next->type) {
 185         case OV2722_8BIT:
 186                 size = 1;
 187                 ctrl->buffer.data[ctrl->index] = (u8)next->val;
 188                 break;
 189         case OV2722_16BIT:
 190                 size = 2;
 191                 data16 = (void *)&ctrl->buffer.data[ctrl->index];
 192                 *data16 = cpu_to_be16((u16)next->val);
 193                 break;
 194         default:
 195                 return -EINVAL;
 196         }
 197
 198         /* When first item is added, we need to store its starting address */
 199         if (ctrl->index == 0)
 200                 ctrl->buffer.addr = next->reg;
 201
 202         ctrl->index += size;
 203
 204         /*
 205          * Buffer cannot guarantee free space for u32? Better flush it to avoid
 206          * possible lack of memory for next item.
 207          */
 208         if (ctrl->index + sizeof(u16) >= OV2722_MAX_WRITE_BUF_SIZE)
 209                 return __ov2722_flush_reg_array(client, ctrl);
 210
 211         return 0;
 212 }
 213
 214 static int __ov2722_write_reg_is_consecutive(struct i2c_client *client,
 215                                              struct ov2722_write_ctrl *ctrl,
 216                                              const struct ov2722_reg *next)
 217 {
 218         if (ctrl->index == 0)
 219                 return 1;
 220
 221         return ctrl->buffer.addr + ctrl->index == next->reg;
 222 }
 223
 224 static int ov2722_write_reg_array(struct i2c_client *client,
 225                                   const struct ov2722_reg *reglist)
 226 {
 227         const struct ov2722_reg *next = reglist;
 228         struct ov2722_write_ctrl ctrl;
 229         int err;
 230
 231         ctrl.index = 0;
 232         for (; next->type != OV2722_TOK_TERM; next++) {
 233                 switch (next->type & OV2722_TOK_MASK) {
 234                 case OV2722_TOK_DELAY:
 235                         err = __ov2722_flush_reg_array(client, &ctrl);
 236                         if (err)
 237                                 return err;
 238                         msleep(next->val);
 239                         break;
 240                 default:
 241                         /*
 242                          * If next address is not consecutive, data needs to be
 243                          * flushed before proceed.
 244                          */
 245                         if (!__ov2722_write_reg_is_consecutive(client, &ctrl,
 246                                                                next)) {
 247                                 err = __ov2722_flush_reg_array(client, &ctrl);
 248                                 if (err)
 249                                         return err;
 250                         }
 251                         err = __ov2722_buf_reg_array(client, &ctrl, next);
 252                         if (err) {
 253                                 dev_err(&client->dev, "%s: write error, aborted\n",
 254                                         __func__);
 255                                 return err;
 256                         }
 257                         break;
 258                 }
 259         }
 260
 261         return __ov2722_flush_reg_array(client, &ctrl);
 262 }
 263
 264 static long __ov2722_set_exposure(struct v4l2_subdev *sd, int coarse_itg,
 265                                   int gain, int digitgain)
 266
 267 {
 268         struct i2c_client *client = v4l2_get_subdevdata(sd);
 269         struct ov2722_device *dev = to_ov2722_sensor(sd);
 270         u16 hts, vts;
 271         int ret;
 272
 273         dev_dbg(&client->dev, "set_exposure without group hold\n");
 274
 275         /* clear VTS_DIFF on manual mode */
 276         ret = ov2722_write_reg(client, OV2722_16BIT, OV2722_VTS_DIFF_H, 0);
 277         if (ret)
 278                 return ret;
 279
 280         hts = dev->pixels_per_line;
 281         vts = dev->lines_per_frame;
 282
 283         if ((coarse_itg + OV2722_COARSE_INTG_TIME_MAX_MARGIN) > vts)
 284                 vts = coarse_itg + OV2722_COARSE_INTG_TIME_MAX_MARGIN;
 285
 286         coarse_itg <<= 4;
 287         digitgain <<= 2;
 288
 289         ret = ov2722_write_reg(client, OV2722_16BIT,
 290                                OV2722_VTS_H, vts);
 291         if (ret)
 292                 return ret;
 293
 294         ret = ov2722_write_reg(client, OV2722_16BIT,
 295                                OV2722_HTS_H, hts);
 296         if (ret)
 297                 return ret;
 298
 299         /* set exposure */
 300         ret = ov2722_write_reg(client, OV2722_8BIT,
 301                                OV2722_AEC_PK_EXPO_L,
 302                                coarse_itg & 0xff);
 303         if (ret)
 304                 return ret;
 305
 306         ret = ov2722_write_reg(client, OV2722_16BIT,
 307                                OV2722_AEC_PK_EXPO_H,
 308                                (coarse_itg >> 8) & 0xfff);
 309         if (ret)
 310                 return ret;
 311
 312         /* set analog gain */
 313         ret = ov2722_write_reg(client, OV2722_16BIT,
 314                                OV2722_AGC_ADJ_H, gain);
 315         if (ret)
 316                 return ret;
 317
 318         /* set digital gain */
 319         ret = ov2722_write_reg(client, OV2722_16BIT,
 320                                OV2722_MWB_GAIN_R_H, digitgain);
 321         if (ret)
 322                 return ret;
 323
 324         ret = ov2722_write_reg(client, OV2722_16BIT,
 325                                OV2722_MWB_GAIN_G_H, digitgain);
 326         if (ret)
 327                 return ret;
 328
 329         ret = ov2722_write_reg(client, OV2722_16BIT,
 330                                OV2722_MWB_GAIN_B_H, digitgain);
 331
 332         return ret;
 333 }
 334
 335 static int ov2722_set_exposure(struct v4l2_subdev *sd, int exposure,
 336                                int gain, int digitgain)
 337 {
 338         struct ov2722_device *dev = to_ov2722_sensor(sd);
 339         int ret;
 340
 341         mutex_lock(&dev->input_lock);
 342         ret = __ov2722_set_exposure(sd, exposure, gain, digitgain);
 343         mutex_unlock(&dev->input_lock);
 344
 345         return ret;
 346 }
 347
 348 static long ov2722_s_exposure(struct v4l2_subdev *sd,
 349                               struct atomisp_exposure *exposure)
 350 {
 351         int exp = exposure->integration_time[0];
 352         int gain = exposure->gain[0];
 353         int digitgain = exposure->gain[1];
 354
 355         /* we should not accept the invalid value below. */
 356         if (gain == 0) {
 357                 struct i2c_client *client = v4l2_get_subdevdata(sd);
 358
 359                 v4l2_err(client, "%s: invalid value\n", __func__);
 360                 return -EINVAL;
 361         }
 362
 363         return ov2722_set_exposure(sd, exp, gain, digitgain);
 364 }
 365
 366 static long ov2722_ioctl(struct v4l2_subdev *sd, unsigned int cmd, void *arg)
 367 {
 368         switch (cmd) {
 369         case ATOMISP_IOC_S_EXPOSURE:
 370                 return ov2722_s_exposure(sd, arg);
 371         default:
 372                 return -EINVAL;
 373         }
 374         return 0;
 375 }
 376
 377 /* This returns the exposure time being used. This should only be used
 378  * for filling in EXIF data, not for actual image processing.
 379  */
 380 static int ov2722_q_exposure(struct v4l2_subdev *sd, s32 *value)
 381 {
 382         struct i2c_client *client = v4l2_get_subdevdata(sd);
 383         u16 reg_v, reg_v2;
 384         int ret;
 385
 386         /* get exposure */
 387         ret = ov2722_read_reg(client, OV2722_8BIT,
 388                               OV2722_AEC_PK_EXPO_L,
 389                               &reg_v);
 390         if (ret)
 391                 goto err;
 392
 393         ret = ov2722_read_reg(client, OV2722_8BIT,
 394                               OV2722_AEC_PK_EXPO_M,
 395                               &reg_v2);
 396         if (ret)
 397                 goto err;
 398
 399         reg_v += reg_v2 << 8;
 400         ret = ov2722_read_reg(client, OV2722_8BIT,
 401                               OV2722_AEC_PK_EXPO_H,
 402                               &reg_v2);
 403         if (ret)
 404                 goto err;
 405
 406         *value = reg_v + (((u32)reg_v2 << 16));
 407 err:
 408         return ret;
 409 }
 410
 411 static int ov2722_g_volatile_ctrl(struct v4l2_ctrl *ctrl)
 412 {
 413         struct ov2722_device *dev =
 414             container_of(ctrl->handler, struct ov2722_device, ctrl_handler);
 415         int ret = 0;
 416         unsigned int val;
 417
 418         switch (ctrl->id) {
 419         case V4L2_CID_EXPOSURE_ABSOLUTE:
 420                 ret = ov2722_q_exposure(&dev->sd, &ctrl->val);
 421                 break;
 422         case V4L2_CID_LINK_FREQ:
 423                 val = dev->res->mipi_freq;
 424                 if (val == 0)
 425                         return -EINVAL;
 426
 427                 ctrl->val = val * 1000; /* To Hz */
 428                 break;
 429         default:
 430                 ret = -EINVAL;
 431         }
 432
 433         return ret;
 434 }
 435
 436 static const struct v4l2_ctrl_ops ctrl_ops = {
 437         .g_volatile_ctrl = ov2722_g_volatile_ctrl
 438 };
 439
 440 static const struct v4l2_ctrl_config ov2722_controls[] = {
 441         {
 442                 .ops = &ctrl_ops,
 443                 .id = V4L2_CID_EXPOSURE_ABSOLUTE,
 444                 .type = V4L2_CTRL_TYPE_INTEGER,
 445                 .name = "exposure",
 446                 .min = 0x0,
 447                 .max = 0xffff,
 448                 .step = 0x01,
 449                 .def = 0x00,
 450                 .flags = 0,
 451         },
 452         {
 453                 .ops = &ctrl_ops,
 454                 .id = V4L2_CID_LINK_FREQ,
 455                 .name = "Link Frequency",
 456                 .type = V4L2_CTRL_TYPE_INTEGER,
 457                 .min = 1,
 458                 .max = 1500000 * 1000,
 459                 .step = 1,
 460                 .def = 1,
 461                 .flags = V4L2_CTRL_FLAG_VOLATILE | V4L2_CTRL_FLAG_READ_ONLY,
 462         },
 463 };
 464
 465 static int ov2722_init(struct v4l2_subdev *sd)
 466 {
 467         struct ov2722_device *dev = to_ov2722_sensor(sd);
 468
 469         mutex_lock(&dev->input_lock);
 470
 471         /* restore settings */
 472         ov2722_res = ov2722_res_preview;
 473         N_RES = N_RES_PREVIEW;
 474
 475         mutex_unlock(&dev->input_lock);
 476
 477         return 0;
 478 }
 479
 480 static int power_ctrl(struct v4l2_subdev *sd, bool flag)
 481 {
 482         int ret = -1;
 483         struct ov2722_device *dev = to_ov2722_sensor(sd);
 484
 485         if (!dev || !dev->platform_data)
 486                 return -ENODEV;
 487
 488         if (flag) {
 489                 ret = dev->platform_data->v1p8_ctrl(sd, 1);
 490                 if (ret == 0) {
 491                         ret = dev->platform_data->v2p8_ctrl(sd, 1);
 492                         if (ret)
 493                                 dev->platform_data->v1p8_ctrl(sd, 0);
 494                 }
 495         } else {
 496                 ret = dev->platform_data->v1p8_ctrl(sd, 0);
 497                 ret |= dev->platform_data->v2p8_ctrl(sd, 0);
 498         }
 499
 500         return ret;
 501 }
 502
 503 static int gpio_ctrl(struct v4l2_subdev *sd, bool flag)
 504 {
 505         struct ov2722_device *dev = to_ov2722_sensor(sd);
 506         int ret = -1;
 507
 508         if (!dev || !dev->platform_data)
 509                 return -ENODEV;
 510
 511         /* Note: the GPIO order is asymmetric: always RESET#
 512          * before PWDN# when turning it on or off.
 513          */
 514         ret = dev->platform_data->gpio0_ctrl(sd, flag);
 515         ret |= dev->platform_data->gpio1_ctrl(sd, flag);
 516         return ret;
 517 }
 518
 519 static int power_up(struct v4l2_subdev *sd)
 520 {
 521         struct ov2722_device *dev = to_ov2722_sensor(sd);
 522         struct i2c_client *client = v4l2_get_subdevdata(sd);
 523         int ret;
 524
 525         if (!dev->platform_data) {
 526                 dev_err(&client->dev,
 527                         "no camera_sensor_platform_data");
 528                 return -ENODEV;
 529         }
 530
 531         /* power control */
 532         ret = power_ctrl(sd, 1);
 533         if (ret)
 534                 goto fail_power;
 535
 536         /* according to DS, at least 5ms is needed between DOVDD and PWDN */
 537         usleep_range(5000, 6000);
 538
 539         /* gpio ctrl */
 540         ret = gpio_ctrl(sd, 1);
 541         if (ret) {
 542                 ret = gpio_ctrl(sd, 0);
 543                 if (ret)
 544                         goto fail_power;
 545         }
 546
 547         /* flis clock control */
 548         ret = dev->platform_data->flisclk_ctrl(sd, 1);
 549         if (ret)
 550                 goto fail_clk;
 551
 552         /* according to DS, 20ms is needed between PWDN and i2c access */
 553         msleep(20);
 554
 555         return 0;
 556
 557 fail_clk:
 558         gpio_ctrl(sd, 0);
 559 fail_power:
 560         power_ctrl(sd, 0);
 561         dev_err(&client->dev, "sensor power-up failed\n");
 562
 563         return ret;
 564 }
 565
 566 static int power_down(struct v4l2_subdev *sd)
 567 {
 568         struct ov2722_device *dev = to_ov2722_sensor(sd);
 569         struct i2c_client *client = v4l2_get_subdevdata(sd);
 570         int ret = 0;
 571
 572         if (!dev->platform_data) {
 573                 dev_err(&client->dev,
 574                         "no camera_sensor_platform_data");
 575                 return -ENODEV;
 576         }
 577
 578         ret = dev->platform_data->flisclk_ctrl(sd, 0);
 579         if (ret)
 580                 dev_err(&client->dev, "flisclk failed\n");
 581
 582         /* gpio ctrl */
 583         ret = gpio_ctrl(sd, 0);
 584         if (ret) {
 585                 ret = gpio_ctrl(sd, 0);
 586                 if (ret)
 587                         dev_err(&client->dev, "gpio failed 2\n");
 588         }
 589
 590         /* power control */
 591         ret = power_ctrl(sd, 0);
 592         if (ret)
 593                 dev_err(&client->dev, "vprog failed.\n");
 594
 595         return ret;
 596 }
 597
 598 static int ov2722_s_power(struct v4l2_subdev *sd, int on)
 599 {
 600         int ret;
 601
 602         if (on == 0)
 603                 return power_down(sd);
 604
 605         ret = power_up(sd);
 606         if (!ret)
 607                 return ov2722_init(sd);
 608
 609         return ret;
 610 }
 611
 612 /* TODO: remove it. */
 613 static int startup(struct v4l2_subdev *sd)
 614 {
 615         struct ov2722_device *dev = to_ov2722_sensor(sd);
 616         struct i2c_client *client = v4l2_get_subdevdata(sd);
 617         int ret = 0;
 618
 619         ret = ov2722_write_reg(client, OV2722_8BIT,
 620                                OV2722_SW_RESET, 0x01);
 621         if (ret) {
 622                 dev_err(&client->dev, "ov2722 reset err.\n");
 623                 return ret;
 624         }
 625
 626         ret = ov2722_write_reg_array(client, dev->res->regs);
 627         if (ret) {
 628                 dev_err(&client->dev, "ov2722 write register err.\n");
 629                 return ret;
 630         }
 631
 632         return ret;
 633 }
 634
 635 static int ov2722_set_fmt(struct v4l2_subdev *sd,
 636                           struct v4l2_subdev_state *sd_state,
 637                           struct v4l2_subdev_format *format)
 638 {
 639         struct v4l2_mbus_framefmt *fmt = &format->format;
 640         struct ov2722_device *dev = to_ov2722_sensor(sd);
 641         struct i2c_client *client = v4l2_get_subdevdata(sd);
 642         struct ov2722_resolution *res;
 643         struct camera_mipi_info *ov2722_info = NULL;
 644         int ret = 0;
 645
 646         if (format->pad)
 647                 return -EINVAL;
 648         if (!fmt)
 649                 return -EINVAL;
 650         ov2722_info = v4l2_get_subdev_hostdata(sd);
 651         if (!ov2722_info)
 652                 return -EINVAL;
 653
 654         res = v4l2_find_nearest_size(ov2722_res_preview,
 655                                      ARRAY_SIZE(ov2722_res_preview), width,
 656                                      height, fmt->width, fmt->height);
 657         if (!res)
 658                 res = &ov2722_res_preview[N_RES - 1];
 659
 660         fmt->width = res->width;
 661         fmt->height = res->height;
 662         dev->res = res;
 663
 664         fmt->code = MEDIA_BUS_FMT_SGRBG10_1X10;
 665         if (format->which == V4L2_SUBDEV_FORMAT_TRY) {
 666                 sd_state->pads->try_fmt = *fmt;
 667                 return 0;
 668         }
 669
 670         mutex_lock(&dev->input_lock);
 671
 672         dev->pixels_per_line = dev->res->pixels_per_line;
 673         dev->lines_per_frame = dev->res->lines_per_frame;
 674
 675         ret = startup(sd);
 676         if (ret) {
 677                 int i = 0;
 678
 679                 dev_err(&client->dev, "ov2722 startup err, retry to power up\n");
 680                 for (i = 0; i < OV2722_POWER_UP_RETRY_NUM; i++) {
 681                         dev_err(&client->dev,
 682                                 "ov2722 retry to power up %d/%d times, result: ",
 683                                 i + 1, OV2722_POWER_UP_RETRY_NUM);
 684                         power_down(sd);
 685                         ret = power_up(sd);
 686                         if (ret) {
 687                                 dev_err(&client->dev, "power up failed, continue\n");
 688                                 continue;
 689                         }
 690                         ret = startup(sd);
 691                         if (ret) {
 692                                 dev_err(&client->dev, " startup FAILED!\n");
 693                         } else {
 694                                 dev_err(&client->dev, " startup SUCCESS!\n");
 695                                 break;
 696                         }
 697                 }
 698                 if (ret) {
 699                         dev_err(&client->dev, "ov2722 startup err\n");
 700                         goto err;
 701                 }
 702         }
 703
 704 err:
 705         mutex_unlock(&dev->input_lock);
 706         return ret;
 707 }
 708
 709 static int ov2722_get_fmt(struct v4l2_subdev *sd,
 710                           struct v4l2_subdev_state *sd_state,
 711                           struct v4l2_subdev_format *format)
 712 {
 713         struct v4l2_mbus_framefmt *fmt = &format->format;
 714         struct ov2722_device *dev = to_ov2722_sensor(sd);
 715
 716         if (format->pad)
 717                 return -EINVAL;
 718         if (!fmt)
 719                 return -EINVAL;
 720
 721         fmt->width = dev->res->width;
 722         fmt->height = dev->res->height;
 723         fmt->code = MEDIA_BUS_FMT_SBGGR10_1X10;
 724
 725         return 0;
 726 }
 727
 728 static int ov2722_detect(struct i2c_client *client)
 729 {
 730         struct i2c_adapter *adapter = client->adapter;
 731         u16 high = 0, low = 0;
 732         u16 id;
 733         u8 revision;
 734
 735         if (!i2c_check_functionality(adapter, I2C_FUNC_I2C))
 736                 return -ENODEV;
 737
 738         ov2722_read_reg(client, OV2722_8BIT,
 739                         OV2722_SC_CMMN_CHIP_ID_H, &high);
 740         ov2722_read_reg(client, OV2722_8BIT,
 741                         OV2722_SC_CMMN_CHIP_ID_L, &low);
 742         id = (high << 8) | low;
 743
 744         if ((id != OV2722_ID) && (id != OV2720_ID)) {
 745                 dev_err(&client->dev, "sensor ID error\n");
 746                 return -ENODEV;
 747         }
 748
 749         high = 0;
 750         ov2722_read_reg(client, OV2722_8BIT,
 751                         OV2722_SC_CMMN_SUB_ID, &high);
 752         revision = (u8)high & 0x0f;
 753
 754         dev_dbg(&client->dev, "sensor_revision = 0x%x\n", revision);
 755         dev_dbg(&client->dev, "detect ov2722 success\n");
 756         return 0;
 757 }
 758
 759 static int ov2722_s_stream(struct v4l2_subdev *sd, int enable)
 760 {
 761         struct ov2722_device *dev = to_ov2722_sensor(sd);
 762         struct i2c_client *client = v4l2_get_subdevdata(sd);
 763         int ret;
 764
 765         mutex_lock(&dev->input_lock);
 766
 767         ret = ov2722_write_reg(client, OV2722_8BIT, OV2722_SW_STREAM,
 768                                enable ? OV2722_START_STREAMING :
 769                                OV2722_STOP_STREAMING);
 770
 771         mutex_unlock(&dev->input_lock);
 772         return ret;
 773 }
 774
 775 static int ov2722_s_config(struct v4l2_subdev *sd,
 776                            int irq, void *platform_data)
 777 {
 778         struct ov2722_device *dev = to_ov2722_sensor(sd);
 779         struct i2c_client *client = v4l2_get_subdevdata(sd);
 780         int ret = 0;
 781
 782         if (!platform_data)
 783                 return -ENODEV;
 784
 785         dev->platform_data =
 786             (struct camera_sensor_platform_data *)platform_data;
 787
 788         mutex_lock(&dev->input_lock);
 789
 790         /* power off the module, then power on it in future
 791          * as first power on by board may not fulfill the
 792          * power on sequqence needed by the module
 793          */
 794         ret = power_down(sd);
 795         if (ret) {
 796                 dev_err(&client->dev, "ov2722 power-off err.\n");
 797                 goto fail_power_off;
 798         }
 799
 800         ret = power_up(sd);
 801         if (ret) {
 802                 dev_err(&client->dev, "ov2722 power-up err.\n");
 803                 goto fail_power_on;
 804         }
 805
 806         ret = dev->platform_data->csi_cfg(sd, 1);
 807         if (ret)
 808                 goto fail_csi_cfg;
 809
 810         /* config & detect sensor */
 811         ret = ov2722_detect(client);
 812         if (ret) {
 813                 dev_err(&client->dev, "ov2722_detect err s_config.\n");
 814                 goto fail_csi_cfg;
 815         }
 816
 817         /* turn off sensor, after probed */
 818         ret = power_down(sd);
 819         if (ret) {
 820                 dev_err(&client->dev, "ov2722 power-off err.\n");
 821                 goto fail_csi_cfg;
 822         }
 823         mutex_unlock(&dev->input_lock);
 824
 825         return 0;
 826
 827 fail_csi_cfg:
 828         dev->platform_data->csi_cfg(sd, 0);
 829 fail_power_on:
 830         power_down(sd);
 831         dev_err(&client->dev, "sensor power-gating failed\n");
 832 fail_power_off:
 833         mutex_unlock(&dev->input_lock);
 834         return ret;
 835 }
 836
 837 static int ov2722_g_frame_interval(struct v4l2_subdev *sd,
 838                                    struct v4l2_subdev_frame_interval *interval)
 839 {
 840         struct ov2722_device *dev = to_ov2722_sensor(sd);
 841
 842         interval->interval.numerator = 1;
 843         interval->interval.denominator = dev->res->fps;
 844
 845         return 0;
 846 }
 847
 848 static int ov2722_enum_mbus_code(struct v4l2_subdev *sd,
 849                                  struct v4l2_subdev_state *sd_state,
 850                                  struct v4l2_subdev_mbus_code_enum *code)
 851 {
 852         if (code->index >= MAX_FMTS)
 853                 return -EINVAL;
 854
 855         code->code = MEDIA_BUS_FMT_SBGGR10_1X10;
 856         return 0;
 857 }
 858
 859 static int ov2722_enum_frame_size(struct v4l2_subdev *sd,
 860                                   struct v4l2_subdev_state *sd_state,
 861                                   struct v4l2_subdev_frame_size_enum *fse)
 862 {
 863         int index = fse->index;
 864
 865         if (index >= N_RES)
 866                 return -EINVAL;
 867
 868         fse->min_width = ov2722_res[index].width;
 869         fse->min_height = ov2722_res[index].height;
 870         fse->max_width = ov2722_res[index].width;
 871         fse->max_height = ov2722_res[index].height;
 872
 873         return 0;
 874 }
 875
 876 static int ov2722_g_skip_frames(struct v4l2_subdev *sd, u32 *frames)
 877 {
 878         struct ov2722_device *dev = to_ov2722_sensor(sd);
 879
 880         mutex_lock(&dev->input_lock);
 881         *frames = dev->res->skip_frames;
 882         mutex_unlock(&dev->input_lock);
 883
 884         return 0;
 885 }
 886
 887 static const struct v4l2_subdev_sensor_ops ov2722_sensor_ops = {
 888         .g_skip_frames  = ov2722_g_skip_frames,
 889 };
 890
 891 static const struct v4l2_subdev_video_ops ov2722_video_ops = {
 892         .s_stream = ov2722_s_stream,
 893         .g_frame_interval = ov2722_g_frame_interval,
 894 };
 895
 896 static const struct v4l2_subdev_core_ops ov2722_core_ops = {
 897         .s_power = ov2722_s_power,
 898         .ioctl = ov2722_ioctl,
 899 };
 900
 901 static const struct v4l2_subdev_pad_ops ov2722_pad_ops = {
 902         .enum_mbus_code = ov2722_enum_mbus_code,
 903         .enum_frame_size = ov2722_enum_frame_size,
 904         .get_fmt = ov2722_get_fmt,
 905         .set_fmt = ov2722_set_fmt,
 906 };
 907
 908 static const struct v4l2_subdev_ops ov2722_ops = {
 909         .core = &ov2722_core_ops,
 910         .video = &ov2722_video_ops,
 911         .pad = &ov2722_pad_ops,
 912         .sensor = &ov2722_sensor_ops,
 913 };
 914
 915 static void ov2722_remove(struct i2c_client *client)
 916 {
 917         struct v4l2_subdev *sd = i2c_get_clientdata(client);
 918         struct ov2722_device *dev = to_ov2722_sensor(sd);
 919
 920         dev->platform_data->csi_cfg(sd, 0);
 921         v4l2_ctrl_handler_free(&dev->ctrl_handler);
 922         v4l2_device_unregister_subdev(sd);
 923
 924         atomisp_gmin_remove_subdev(sd);
 925
 926         media_entity_cleanup(&dev->sd.entity);
 927         kfree(dev);
 928 }
 929
 930 static int __ov2722_init_ctrl_handler(struct ov2722_device *dev)
 931 {
 932         struct v4l2_ctrl_handler *hdl;
 933         unsigned int i;
 934
 935         hdl = &dev->ctrl_handler;
 936         v4l2_ctrl_handler_init(&dev->ctrl_handler, ARRAY_SIZE(ov2722_controls));
 937         for (i = 0; i < ARRAY_SIZE(ov2722_controls); i++)
 938                 v4l2_ctrl_new_custom(&dev->ctrl_handler, &ov2722_controls[i],
 939                                      NULL);
 940
 941         dev->link_freq = v4l2_ctrl_find(&dev->ctrl_handler, V4L2_CID_LINK_FREQ);
 942
 943         if (dev->ctrl_handler.error || !dev->link_freq)
 944                 return dev->ctrl_handler.error;
 945
 946         dev->sd.ctrl_handler = hdl;
 947
 948         return 0;
 949 }
 950
 951 static int ov2722_probe(struct i2c_client *client)
 952 {
 953         struct ov2722_device *dev;
 954         void *ovpdev;
 955         int ret;
 956
 957         dev = kzalloc(sizeof(*dev), GFP_KERNEL);
 958         if (!dev)
 959                 return -ENOMEM;
 960
 961         mutex_init(&dev->input_lock);
 962
 963         dev->res = &ov2722_res_preview[0];
 964         v4l2_i2c_subdev_init(&dev->sd, client, &ov2722_ops);
 965
 966         ovpdev = gmin_camera_platform_data(&dev->sd,
 967                                            ATOMISP_INPUT_FORMAT_RAW_10,
 968                                            atomisp_bayer_order_grbg);
 969
 970         ret = ov2722_s_config(&dev->sd, client->irq, ovpdev);
 971         if (ret)
 972                 goto out_free;
 973
 974         ret = __ov2722_init_ctrl_handler(dev);
 975         if (ret)
 976                 goto out_ctrl_handler_free;
 977
 978         dev->sd.flags |= V4L2_SUBDEV_FL_HAS_DEVNODE;
 979         dev->pad.flags = MEDIA_PAD_FL_SOURCE;
 980         dev->format.code = MEDIA_BUS_FMT_SBGGR10_1X10;
 981         dev->sd.entity.function = MEDIA_ENT_F_CAM_SENSOR;
 982
 983         ret = media_entity_pads_init(&dev->sd.entity, 1, &dev->pad);
 984         if (ret)
 985                 ov2722_remove(client);
 986
 987         return atomisp_register_i2c_module(&dev->sd, ovpdev, RAW_CAMERA);
 988
 989 out_ctrl_handler_free:
 990         v4l2_ctrl_handler_free(&dev->ctrl_handler);
 991
 992 out_free:
 993         atomisp_gmin_remove_subdev(&dev->sd);
 994         v4l2_device_unregister_subdev(&dev->sd);
 995         kfree(dev);
 996         return ret;
 997 }
 998
 999 static const struct acpi_device_id ov2722_acpi_match[] = {
1000         { "INT33FB" },
1001         {},
1002 };
1003 MODULE_DEVICE_TABLE(acpi, ov2722_acpi_match);
1004
1005 static struct i2c_driver ov2722_driver = {
1006         .driver = {
1007                 .name = "ov2722",
1008                 .acpi_match_table = ov2722_acpi_match,
1009         },
1010         .probe_new = ov2722_probe,
1011         .remove = ov2722_remove,
1012 };
1013 module_i2c_driver(ov2722_driver);
1014
1015 MODULE_AUTHOR("Wei Liu <wei.liu@intel.com>");
1016 MODULE_DESCRIPTION("A low-level driver for OmniVision 2722 sensors");
1017 MODULE_LICENSE("GPL");