drivers/media/platform/tegra/camera/camera_common.c

   1 /*
   2  * camera_common.c - utilities for tegra camera driver
   3  *
   4  * Copyright (c) 2015-2016, NVIDIA CORPORATION.  All rights reserved.
   5  *
   6  * This program is free software; you can redistribute it and/or modify it
   7  * under the terms and conditions of the GNU General Public License,
   8  * version 2, as published by the Free Software Foundation.
   9  *
  10  * This program is distributed in the hope it will be useful, but WITHOUT
  11  * ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or
  12  * FITNESS FOR A PARTICULAR PURPOSE.  See the GNU General Public License for
  13  * more details.
  14  *
  15  * You should have received a copy of the GNU General Public License
  16  * along with this program.  If not, see <http://www.gnu.org/licenses/>.
  17  */
  18
  19 #include <media/camera_common.h>
  20 #include <linux/of_graph.h>
  21 #include <linux/string.h>
  22 #include <mach/io_dpd.h>
  23
  24 #define has_s_op(master, op) \
  25         (master->ops && master->ops->op)
  26 #define call_s_op(master, op) \
  27         (has_s_op(master, op) ? \
  28          master->ops->op(master) : 0)
  29 #define call_s_ops(master, op, ...) \
  30         (has_s_op(master, op) ? \
  31          master->ops->op(master, __VA_ARGS__) : 0)
  32
  33 static const struct camera_common_colorfmt camera_common_color_fmts[] = {
  34         {
  35                 V4L2_MBUS_FMT_SRGGB12_1X12,
  36                 V4L2_COLORSPACE_SRGB,
  37                 V4L2_PIX_FMT_SRGGB12,
  38         },
  39         {
  40                 V4L2_MBUS_FMT_SRGGB10_1X10,
  41                 V4L2_COLORSPACE_SRGB,
  42                 V4L2_PIX_FMT_SRGGB10,
  43         },
  44         {
  45                 V4L2_MBUS_FMT_SRGGB8_1X8,
  46                 V4L2_COLORSPACE_SRGB,
  47                 V4L2_PIX_FMT_SRGGB8,
  48         },
  49 };
  50
  51 static struct tegra_io_dpd camera_common_csi_io[] = {
  52         {
  53                 .name                   = "CSIA",
  54                 .io_dpd_reg_index       = 0,
  55                 .io_dpd_bit             = 0x0,
  56         },
  57         {
  58                 .name                   = "CSIB",
  59                 .io_dpd_reg_index       = 0,
  60                 .io_dpd_bit             = 0x1,
  61         },
  62         {
  63                 .name                   = "CSIC",
  64                 .io_dpd_reg_index       = 1,
  65                 .io_dpd_bit             = 0xa,
  66         },
  67         {
  68                 .name                   = "CSID",
  69                 .io_dpd_reg_index       = 1,
  70                 .io_dpd_bit             = 0xb,
  71         },
  72         {
  73                 .name                   = "CSIE",
  74                 .io_dpd_reg_index       = 1,
  75                 .io_dpd_bit             = 0xc,
  76         },
  77         {
  78                 .name                   = "CSIF",
  79                 .io_dpd_reg_index       = 1,
  80                 .io_dpd_bit             = 0xd,
  81         },
  82 };
  83
  84 int camera_common_g_ctrl(struct camera_common_data *s_data,
  85                          struct v4l2_control *control)
  86 {
  87         int i;
  88
  89         for (i = 0; i < s_data->numctrls; i++) {
  90                 if (s_data->ctrls[i]->id == control->id) {
  91                         control->value = s_data->ctrls[i]->val;
  92                         dev_dbg(&s_data->i2c_client->dev,
  93                                  "%s: found control %s\n", __func__,
  94                                  s_data->ctrls[i]->name);
  95                         return 0;
  96                 }
  97         }
  98
  99         return -EINVAL;
 100 }
 101 EXPORT_SYMBOL(camera_common_g_ctrl);
 102
 103 int camera_common_regulator_get(struct i2c_client *client,
 104                        struct regulator **vreg, const char *vreg_name)
 105 {
 106         struct regulator *reg = NULL;
 107         int err = 0;
 108
 109         reg = devm_regulator_get(&client->dev, vreg_name);
 110         if (unlikely(IS_ERR(reg))) {
 111                 dev_err(&client->dev, "%s %s ERR: %p\n",
 112                         __func__, vreg_name, reg);
 113                 err = PTR_ERR(reg);
 114                 reg = NULL;
 115         } else
 116                 dev_dbg(&client->dev, "%s: %s\n",
 117                         __func__, vreg_name);
 118
 119         *vreg = reg;
 120         return err;
 121 }
 122 EXPORT_SYMBOL(camera_common_regulator_get);
 123
 124 int camera_common_parse_clocks(struct i2c_client *client,
 125                         struct camera_common_pdata *pdata)
 126 {
 127         struct device_node *np = client->dev.of_node;
 128         const char *prop;
 129         int proplen = 0;
 130         int i = 0;
 131         int numclocks = 0;
 132         int mclk_index = 0;
 133         int parentclk_index = -1;
 134         int err = 0;
 135
 136
 137         pdata->mclk_name = NULL;
 138         pdata->parentclk_name = NULL;
 139         err = of_property_read_string(np, "mclk", &pdata->mclk_name);
 140         if (!err) {
 141                 dev_dbg(&client->dev, "mclk in DT %s\n", pdata->mclk_name);
 142                 of_property_read_string(np, "parent-clk",
 143                                               &pdata->parentclk_name);
 144                 return 0;
 145         }
 146
 147         prop = (const char *)of_get_property(np, "clock-names", &proplen);
 148         if (prop == NULL)
 149                 return -ENODATA;
 150
 151         /* find length of clock-names string array */
 152         for (i = 0; i < proplen; i++) {
 153                 if (prop[i] == '\0')
 154                         numclocks++;
 155         }
 156
 157         if (numclocks > 1) {
 158                 err = of_property_read_u32(np, "mclk-index", &mclk_index);
 159                 if (err) {
 160                         dev_err(&client->dev, "Failed to find mclk index\n");
 161                         return err;
 162                 }
 163                 err = of_property_read_u32(np, "parent-clk-index",
 164                                            &parentclk_index);
 165         }
 166
 167         for (i = 0; i < numclocks; i++) {
 168                 if (i == mclk_index) {
 169                         pdata->mclk_name = prop;
 170                         dev_dbg(&client->dev, "%s: mclk_name is %s\n",
 171                                  __func__, pdata->mclk_name);
 172                 } else if (i == parentclk_index) {
 173                         pdata->parentclk_name = prop;
 174                         dev_dbg(&client->dev, "%s: parentclk_name is %s\n",
 175                                  __func__, pdata->parentclk_name);
 176                 } else
 177                         dev_dbg(&client->dev, "%s: %s\n", __func__, prop);
 178                 prop += strlen(prop) + 1;
 179         }
 180
 181         return 0;
 182 }
 183
 184 int camera_common_parse_ports(struct i2c_client *client,
 185                               struct camera_common_data *s_data)
 186 {
 187         struct device_node *node = client->dev.of_node;
 188         struct device_node *ep = NULL;
 189         struct device_node *next;
 190         int bus_width = 0;
 191         int err = 0;
 192         int port = 0;
 193
 194         /* Parse all the remote entities and put them into the list */
 195         next = of_graph_get_next_endpoint(node, ep);
 196         if (next == NULL)
 197                 return -ENODATA;
 198
 199         of_node_put(ep);
 200         ep = next;
 201
 202         err = of_property_read_u32(ep, "bus-width", &bus_width);
 203         if (err) {
 204                 dev_err(&client->dev,
 205                         "Failed to find num of lanes\n");
 206                 return err;
 207         }
 208         s_data->numlanes = bus_width;
 209
 210         err = of_property_read_u32(ep, "csi-port", &port);
 211         if (err) {
 212                 dev_err(&client->dev,
 213                         "Failed to find CSI port\n");
 214                 return err;
 215         }
 216         s_data->csi_port = port;
 217
 218         dev_dbg(&client->dev, "%s: csi port %d num of lanes %d\n",
 219                 __func__, s_data->csi_port, s_data->numlanes);
 220
 221         return 0;
 222 }
 223
 224 int camera_common_debugfs_show(struct seq_file *s, void *unused)
 225 {
 226         struct camera_common_data *s_data = s->private;
 227
 228         dev_dbg(&s_data->i2c_client->dev, "%s: ++\n", __func__);
 229
 230         return 0;
 231 }
 232
 233 ssize_t camera_common_debugfs_write(
 234         struct file *file,
 235         char const __user *buf,
 236         size_t count,
 237         loff_t *offset)
 238 {
 239         struct camera_common_data *s_data =
 240                 ((struct seq_file *)file->private_data)->private;
 241         struct i2c_client *client = s_data->i2c_client;
 242         int err = 0;
 243         char buffer[MAX_BUFFER_SIZE];
 244         u32 address;
 245         u32 data;
 246         u8 readback = 0;
 247
 248         dev_dbg(&client->dev, "%s: ++\n", __func__);
 249
 250         if (copy_from_user(&buffer, buf, sizeof(buffer)))
 251                 goto debugfs_write_fail;
 252
 253         if (sscanf(buf, "0x%x 0x%x", &address, &data) == 2)
 254                 goto set_attr;
 255         if (sscanf(buf, "0X%x 0X%x", &address, &data) == 2)
 256                 goto set_attr;
 257         if (sscanf(buf, "%d %d", &address, &data) == 2)
 258                 goto set_attr;
 259
 260         if (sscanf(buf, "0x%x 0x%x", &address, &data) == 1)
 261                 goto read;
 262         if (sscanf(buf, "0X%x 0X%x", &address, &data) == 1)
 263                 goto read;
 264         if (sscanf(buf, "%d %d", &address, &data) == 1)
 265                 goto read;
 266
 267         dev_err(&client->dev, "SYNTAX ERROR: %s\n", buf);
 268         return -EFAULT;
 269
 270 set_attr:
 271         dev_dbg(&client->dev,
 272                         "new address = %x, data = %x\n", address, data);
 273         err |= call_s_ops(s_data, write_reg, address, data);
 274 read:
 275         err |= call_s_ops(s_data, read_reg, address, &readback);
 276         dev_dbg(&client->dev,
 277                         "wrote to address 0x%x with value 0x%x\n",
 278                         address, readback);
 279
 280         if (err)
 281                 goto debugfs_write_fail;
 282
 283         return count;
 284
 285 debugfs_write_fail:
 286         dev_err(&client->dev,
 287                         "%s: test pattern write failed\n", __func__);
 288         return -EFAULT;
 289 }
 290
 291 int camera_common_debugfs_open(struct inode *inode, struct file *file)
 292 {
 293         struct camera_common_data *s_data = inode->i_private;
 294         struct i2c_client *client = s_data->i2c_client;
 295
 296         dev_dbg(&client->dev, "%s: ++\n", __func__);
 297
 298         return single_open(file, camera_common_debugfs_show, inode->i_private);
 299 }
 300
 301 static const struct file_operations camera_common_debugfs_fops = {
 302         .open           = camera_common_debugfs_open,
 303         .read           = seq_read,
 304         .write          = camera_common_debugfs_write,
 305         .llseek         = seq_lseek,
 306         .release        = single_release,
 307 };
 308
 309 void camera_common_remove_debugfs(
 310                 struct camera_common_data *s_data)
 311 {
 312         struct i2c_client *client = s_data->i2c_client;
 313
 314         dev_dbg(&client->dev, "%s: ++\n", __func__);
 315
 316         debugfs_remove_recursive(s_data->debugdir);
 317         s_data->debugdir = NULL;
 318 }
 319 EXPORT_SYMBOL(camera_common_remove_debugfs);
 320
 321 void camera_common_create_debugfs(
 322                 struct camera_common_data *s_data,
 323                 const char *name)
 324 {
 325         struct dentry *err;
 326         struct i2c_client *client = s_data->i2c_client;
 327
 328         dev_dbg(&client->dev, "%s %s\n", __func__, name);
 329
 330         s_data->debugdir =
 331                 debugfs_create_dir(name, NULL);
 332         if (!s_data->debugdir)
 333                 goto remove_debugfs;
 334
 335         err = debugfs_create_file("d",
 336                                 S_IWUSR | S_IRUGO,
 337                                 s_data->debugdir, s_data,
 338                                 &camera_common_debugfs_fops);
 339         if (!err)
 340                 goto remove_debugfs;
 341
 342         return;
 343 remove_debugfs:
 344         dev_err(&client->dev, "couldn't create debugfs\n");
 345         camera_common_remove_debugfs(s_data);
 346 }
 347 EXPORT_SYMBOL(camera_common_create_debugfs);
 348
 349 /* Find a data format by a pixel code in an array */
 350 const struct camera_common_colorfmt *camera_common_find_datafmt(
 351                 enum v4l2_mbus_pixelcode code)
 352 {
 353         int i;
 354
 355         for (i = 0; i < ARRAY_SIZE(camera_common_color_fmts); i++)
 356                 if (camera_common_color_fmts[i].code == code)
 357                         return camera_common_color_fmts + i;
 358
 359         return NULL;
 360 }
 361 EXPORT_SYMBOL(camera_common_find_datafmt);
 362
 363 int camera_common_enum_mbus_code(struct v4l2_subdev *sd,
 364                                 struct v4l2_subdev_fh *fh,
 365                                 struct v4l2_subdev_mbus_code_enum *code)
 366 {
 367         struct i2c_client *client = v4l2_get_subdevdata(sd);
 368         struct camera_common_data *s_data = to_camera_common_data(client);
 369
 370         if (s_data->num_color_fmts < 1 || !s_data->color_fmts) {
 371                 s_data->color_fmts = camera_common_color_fmts;
 372                 s_data->num_color_fmts = ARRAY_SIZE(camera_common_color_fmts);
 373         }
 374
 375         if ((unsigned int)code->index >= s_data->num_color_fmts)
 376                 return -EINVAL;
 377
 378         code->code = s_data->color_fmts[code->index].code;
 379         return 0;
 380 }
 381 EXPORT_SYMBOL(camera_common_enum_mbus_code);
 382
 383 int camera_common_enum_fmt(struct v4l2_subdev *sd, unsigned int index,
 384                          enum v4l2_mbus_pixelcode *code)
 385 {
 386         struct i2c_client *client = v4l2_get_subdevdata(sd);
 387         struct camera_common_data *s_data = to_camera_common_data(client);
 388
 389         if (s_data->num_color_fmts < 1 || !s_data->color_fmts) {
 390                 s_data->color_fmts = camera_common_color_fmts;
 391                 s_data->num_color_fmts = ARRAY_SIZE(camera_common_color_fmts);
 392         }
 393
 394         if ((unsigned int)index >= s_data->num_color_fmts)
 395                 return -EINVAL;
 396
 397         *code = s_data->color_fmts[index].code;
 398         return 0;
 399 }
 400 EXPORT_SYMBOL(camera_common_enum_fmt);
 401
 402 int camera_common_try_fmt(struct v4l2_subdev *sd, struct v4l2_mbus_framefmt *mf)
 403 {
 404         struct i2c_client *client = v4l2_get_subdevdata(sd);
 405         struct camera_common_data *s_data = to_camera_common_data(client);
 406         struct v4l2_control hdr_control;
 407         const struct camera_common_frmfmt *frmfmt = s_data->frmfmt;
 408         int hdr_en;
 409         int err = 0;
 410         int i;
 411
 412         dev_dbg(&client->dev, "%s: size %i x %i\n", __func__,
 413                  mf->width, mf->height);
 414
 415         /* check hdr enable ctrl */
 416         hdr_control.id = V4L2_CID_HDR_EN;
 417
 418         err = v4l2_g_ctrl(s_data->ctrl_handler, &hdr_control);
 419         if (err < 0) {
 420                 dev_err(&client->dev, "could not find device ctrl.\n");
 421                 return err;
 422         }
 423
 424         hdr_en = switch_ctrl_qmenu[hdr_control.value];
 425
 426         s_data->mode = s_data->def_mode;
 427         s_data->fmt_width = s_data->def_width;
 428         s_data->fmt_height = s_data->def_height;
 429         s_data->fmt_maxfps = s_data->def_maxfps;
 430
 431         if (s_data->use_sensor_mode_id &&
 432                 s_data->sensor_mode_id >= 0 &&
 433                 s_data->sensor_mode_id < s_data->numfmts) {
 434                 dev_dbg(&client->dev, "%s: use_sensor_mode_id %d\n",
 435                                 __func__, s_data->sensor_mode_id);
 436                 s_data->mode = frmfmt[s_data->sensor_mode_id].mode;
 437                 s_data->fmt_width = mf->width;
 438                 s_data->fmt_height = mf->height;
 439                 s_data->fmt_maxfps = mf->maxframerate;
 440         } else {
 441                 for (i = 0; i < s_data->numfmts; i++) {
 442                         if (mf->width == frmfmt[i].size.width &&
 443                                 mf->height == frmfmt[i].size.height &&
 444                                 mf->maxframerate == frmfmt[i].framerates[0] &&
 445                                 hdr_en == frmfmt[i].hdr_en) {
 446                                 s_data->mode = frmfmt[i].mode;
 447                                 s_data->fmt_width = mf->width;
 448                                 s_data->fmt_height = mf->height;
 449                                 s_data->fmt_maxfps = mf->maxframerate;
 450                                 break;
 451                         }
 452                 }
 453
 454                 if (i == s_data->numfmts) {
 455                         mf->width = s_data->fmt_width;
 456                         mf->height = s_data->fmt_height;
 457                         mf->maxframerate = s_data->fmt_maxfps;
 458                         dev_dbg(&client->dev,
 459                                 "%s: invalid resolution supplied(set mode) %d %d %d\n",
 460                                 __func__, mf->width,
 461                                 mf->height, mf->maxframerate);
 462                 }
 463         }
 464         if (mf->code != V4L2_MBUS_FMT_SRGGB8_1X8 &&
 465                 mf->code != V4L2_MBUS_FMT_SRGGB10_1X10) {
 466                 mf->code = V4L2_MBUS_FMT_SRGGB10_1X10;
 467                 err = -EINVAL;
 468         }
 469
 470         mf->field = V4L2_FIELD_NONE;
 471         mf->colorspace = V4L2_COLORSPACE_SRGB;
 472
 473         return err;
 474 }
 475 EXPORT_SYMBOL(camera_common_try_fmt);
 476
 477 int camera_common_s_fmt(struct v4l2_subdev *sd, struct v4l2_mbus_framefmt *mf)
 478 {
 479         struct i2c_client *client = v4l2_get_subdevdata(sd);
 480         struct camera_common_data *s_data = to_camera_common_data(client);
 481         int ret;
 482
 483         dev_dbg(&client->dev, "%s(%u) size %i x %i\n", __func__,
 484                         mf->code, mf->width, mf->height);
 485
 486         /* MIPI CSI could have changed the format, double-check */
 487         if (!camera_common_find_datafmt(mf->code))
 488                 return -EINVAL;
 489
 490         ret = camera_common_try_fmt(sd, mf);
 491
 492         s_data->colorfmt = camera_common_find_datafmt(mf->code);
 493
 494         return ret;
 495 }
 496 EXPORT_SYMBOL(camera_common_s_fmt);
 497
 498 int camera_common_g_fmt(struct v4l2_subdev *sd, struct v4l2_mbus_framefmt *mf)
 499 {
 500         struct i2c_client *client = v4l2_get_subdevdata(sd);
 501         struct camera_common_data *s_data = to_camera_common_data(client);
 502         const struct camera_common_colorfmt *fmt = s_data->colorfmt;
 503
 504         dev_dbg(&client->dev, "%s++\n", __func__);
 505
 506         mf->code                = fmt->code;
 507         mf->colorspace          = fmt->colorspace;
 508         mf->width               = s_data->fmt_width;
 509         mf->height              = s_data->fmt_height;
 510         mf->field               = V4L2_FIELD_NONE;
 511         mf->maxframerate        = s_data->fmt_maxfps;
 512
 513         return 0;
 514 }
 515 EXPORT_SYMBOL(camera_common_g_fmt);
 516
 517 static int camera_common_evaluate_color_format(struct v4l2_subdev *sd,
 518                                                int pixelformat)
 519 {
 520         struct i2c_client *client = v4l2_get_subdevdata(sd);
 521         struct camera_common_data *s_data = to_camera_common_data(client);
 522         int i;
 523
 524         if (!s_data)
 525                 return -EINVAL;
 526
 527         if (s_data->num_color_fmts < 1 || !s_data->color_fmts) {
 528                 s_data->color_fmts = camera_common_color_fmts;
 529                 s_data->num_color_fmts = ARRAY_SIZE(camera_common_color_fmts);
 530         }
 531
 532         for (i = 0; i < s_data->num_color_fmts; i++) {
 533                 if (s_data->color_fmts[i].pix_fmt == pixelformat)
 534                         break;
 535         }
 536
 537         if (i >= s_data->num_color_fmts)
 538                 return -EINVAL;
 539
 540         return 0;
 541 }
 542
 543 int camera_common_enum_framesizes(struct v4l2_subdev *sd,
 544                                   struct v4l2_frmsizeenum *fsizes)
 545 {
 546         struct i2c_client *client = v4l2_get_subdevdata(sd);
 547         struct camera_common_data *s_data = to_camera_common_data(client);
 548         int ret;
 549
 550         if (!s_data || !s_data->frmfmt)
 551                 return -EINVAL;
 552
 553         if (fsizes->index >= s_data->numfmts)
 554                 return -EINVAL;
 555
 556         ret = camera_common_evaluate_color_format(sd, fsizes->pixel_format);
 557         if (ret)
 558                 return ret;
 559
 560         fsizes->type = V4L2_FRMSIZE_TYPE_DISCRETE;
 561         fsizes->discrete = s_data->frmfmt[fsizes->index].size;
 562
 563         return 0;
 564 }
 565 EXPORT_SYMBOL(camera_common_enum_framesizes);
 566
 567 int camera_common_enum_frameintervals(struct v4l2_subdev *sd,
 568                                       struct v4l2_frmivalenum *fintervals)
 569 {
 570         struct i2c_client *client = v4l2_get_subdevdata(sd);
 571         struct camera_common_data *s_data = to_camera_common_data(client);
 572         int i, ret;
 573
 574         if (!s_data || !s_data->frmfmt)
 575                 return -EINVAL;
 576
 577         /* Check color format */
 578         ret = camera_common_evaluate_color_format(sd, fintervals->pixel_format);
 579         if (ret)
 580                 return ret;
 581
 582         /* Check resolution sizes */
 583         for (i = 0; i < s_data->numfmts; i++) {
 584                 if (s_data->frmfmt[i].size.width == fintervals->width &&
 585                     s_data->frmfmt[i].size.height == fintervals->height)
 586                         break;
 587         }
 588         if (i >= s_data->numfmts)
 589                 return -EINVAL;
 590
 591         /* Check index is in the rage of framerates array index */
 592         if (fintervals->index >= s_data->frmfmt[i].num_framerates)
 593                 return -EINVAL;
 594
 595         fintervals->type = V4L2_FRMSIZE_TYPE_DISCRETE;
 596         fintervals->discrete.numerator = 1;
 597         fintervals->discrete.denominator =
 598                 s_data->frmfmt[i].framerates[fintervals->index];
 599
 600         return 0;
 601 }
 602 EXPORT_SYMBOL(camera_common_enum_frameintervals);
 603
 604 static void camera_common_mclk_disable(struct camera_common_data *s_data)
 605 {
 606         struct camera_common_power_rail *pw = s_data->power;
 607
 608         if (!pw) {
 609                 dev_err(&s_data->i2c_client->dev, "%s: no device power rail\n",
 610                         __func__);
 611                 return;
 612         }
 613
 614         dev_dbg(&s_data->i2c_client->dev, "%s: disable MCLK\n", __func__);
 615         clk_disable_unprepare(pw->mclk);
 616 }
 617
 618 static int camera_common_mclk_enable(struct camera_common_data *s_data)
 619 {
 620         int err;
 621         struct camera_common_power_rail *pw = s_data->power;
 622         unsigned long mclk_init_rate = s_data->def_clk_freq;
 623
 624         if (!pw) {
 625                 dev_err(s_data->dev, "%s: no device power rail\n",
 626                         __func__);
 627                 return -ENODEV;
 628         }
 629
 630         dev_dbg(s_data->dev, "%s: enable MCLK with %lu Hz\n",
 631                 __func__, mclk_init_rate);
 632
 633         err = clk_set_rate(pw->mclk, mclk_init_rate);
 634         if (!err)
 635                 err = clk_prepare_enable(pw->mclk);
 636         return err;
 637 }
 638
 639 void camera_common_dpd_disable(struct camera_common_data *s_data)
 640 {
 641         int i;
 642         int io_idx;
 643         /* 2 lanes per port, divide by two to get numports */
 644         int numports = (s_data->numlanes + 1) >> 1;
 645
 646         /* disable CSI IOs DPD mode to turn on camera */
 647         for (i = 0; i < numports; i++) {
 648                 io_idx = s_data->csi_port + i;
 649                 tegra_io_dpd_disable(&camera_common_csi_io[io_idx]);
 650                 dev_dbg(s_data->dev,
 651                          "%s: csi %d\n", __func__, io_idx);
 652         }
 653 }
 654
 655 void camera_common_dpd_enable(struct camera_common_data *s_data)
 656 {
 657         int i;
 658         int io_idx;
 659         /* 2 lanes per port, divide by two to get numports */
 660         int numports = (s_data->numlanes + 1) >> 1;
 661
 662         /* disable CSI IOs DPD mode to turn on camera */
 663         for (i = 0; i < numports; i++) {
 664                 io_idx = s_data->csi_port + i;
 665                 tegra_io_dpd_enable(&camera_common_csi_io[io_idx]);
 666                 dev_dbg(s_data->dev,
 667                          "%s: csi %d\n", __func__, io_idx);
 668         }
 669 }
 670
 671 int camera_common_s_power(struct v4l2_subdev *sd, int on)
 672 {
 673         int err = 0;
 674         struct i2c_client *client = v4l2_get_subdevdata(sd);
 675         struct camera_common_data *s_data = to_camera_common_data(client);
 676
 677         if (on) {
 678                 err = camera_common_mclk_enable(s_data);
 679                 if (err)
 680                         return err;
 681
 682                 err = call_s_op(s_data, power_on);
 683                 if (err) {
 684                         dev_err(s_data->dev,
 685                                 "%s: error power on\n", __func__);
 686                         camera_common_mclk_disable(s_data);
 687                 }
 688         } else {
 689                 call_s_op(s_data, power_off);
 690                 camera_common_mclk_disable(s_data);
 691         }
 692
 693         return err;
 694 }
 695 EXPORT_SYMBOL(camera_common_s_power);
 696
 697 int camera_common_g_mbus_config(struct v4l2_subdev *sd,
 698                                 struct v4l2_mbus_config *cfg)
 699 {
 700         cfg->type = V4L2_MBUS_CSI2;
 701         cfg->flags = V4L2_MBUS_CSI2_4_LANE |
 702                 V4L2_MBUS_CSI2_CHANNEL_0 |
 703                 V4L2_MBUS_CSI2_CONTINUOUS_CLOCK;
 704
 705         return 0;
 706 }
 707 EXPORT_SYMBOL(camera_common_g_mbus_config);
 708
 709 int camera_common_focuser_s_power(struct v4l2_subdev *sd, int on)
 710 {
 711         int err = 0;
 712         struct i2c_client *client = v4l2_get_subdevdata(sd);
 713         struct camera_common_focuser_data *s_data =
 714                         to_camera_common_focuser_data(client);
 715
 716         if (on) {
 717                 err = call_s_op(s_data, power_on);
 718                 if (err)
 719                         dev_err(&s_data->i2c_client->dev,
 720                                 "%s: error power on\n", __func__);
 721         } else
 722                 err = call_s_op(s_data, power_off);
 723
 724         return err;
 725 }
 726
 727 int camera_common_focuser_init(struct camera_common_focuser_data *s_data)
 728 {
 729         int err = 0;
 730
 731         /* power on */
 732         err = call_s_op(s_data, power_on);
 733         if (err) {
 734                 dev_err(&s_data->i2c_client->dev,
 735                         "%s: error power on\n", __func__);
 736                 return err;
 737         }
 738
 739         /* load default configuration */
 740         err = call_s_op(s_data, load_config);
 741         if (err) {
 742                 dev_err(&s_data->i2c_client->dev,
 743                         "%s: error loading config\n", __func__);
 744                 goto fail;
 745         }
 746
 747         /* set controls */
 748         err = call_s_op(s_data, ctrls_init);
 749         if (err)
 750                 dev_err(&s_data->i2c_client->dev,
 751                         "%s: error initializing controls\n", __func__);
 752
 753 fail:
 754         /* power off */
 755         err |= call_s_op(s_data, power_off);
 756
 757         return err;
 758 }
 759
 760 int camera_common_parse_sensor_mode(struct i2c_client *client,
 761                         struct camera_common_pdata *pdata)
 762 {
 763         struct device_node *np = client->dev.of_node;
 764         char temp_str[OF_MAX_STR_LEN];
 765         const char *str;
 766         struct device_node *node;
 767         int num_modes = 0;
 768         int err, i;
 769
 770         /* get number of modes */
 771         for (i = 0; num_modes < MAX_NUM_SENSOR_MODES; i++) {
 772                 snprintf(temp_str, sizeof(temp_str), "%s%d",
 773                         OF_SENSORMODE_PREFIX, i);
 774                 node = of_find_node_by_name(np, temp_str);
 775                 if (node == NULL)
 776                         break;
 777                 num_modes++;
 778         }
 779
 780         pdata->mode_info = devm_kzalloc(&client->dev,
 781                 num_modes * sizeof(struct camera_common_mode_info),
 782                 GFP_KERNEL);
 783         if (!pdata->mode_info) {
 784                 dev_err(&client->dev, "Failed to allocate memory for mode info\n");
 785                 return -ENOMEM;
 786         }
 787         memset(pdata->mode_info, 0, num_modes *
 788                sizeof(struct camera_common_mode_info));
 789
 790         /* parse mode info */
 791         for (i = 0; i < num_modes; i++) {
 792                 snprintf(temp_str, sizeof(temp_str), "%s%d",
 793                         OF_SENSORMODE_PREFIX, i);
 794                 node = of_find_node_by_name(np, temp_str);
 795                 if (node == NULL) {
 796                         dev_err(&client->dev, "Failed to find mode\n");
 797                         return -ENODATA;
 798                 };
 799
 800                 /* read mode width */
 801                 of_property_read_string(node, "active_w", &str);
 802                 if (str == NULL) {
 803                         dev_err(&client->dev, "Failed to read mode width\n");
 804                         return -ENODATA;
 805                 };
 806                 err = kstrtoint(str, 10, &pdata->mode_info[i].width);
 807                 if (err) {
 808                         dev_err(&client->dev, "Failed to convert mode width\n");
 809                         return -EFAULT;
 810                 }
 811                 /* read mode height */
 812                 of_property_read_string(node, "active_h", &str);
 813                 if (str == NULL) {
 814                         dev_err(&client->dev, "Failed to read mode height\n");
 815                         return -ENODATA;
 816                 };
 817                 err = kstrtoint(str, 10, &pdata->mode_info[i].height);
 818                 if (err) {
 819                         dev_err(&client->dev, "Failed to convert mode height\n");
 820                         return -EFAULT;
 821                 }
 822                 dev_info(&client->dev, "%s: mode %d x %d:\n", __func__,
 823                         pdata->mode_info[i].width, pdata->mode_info[i].height);
 824
 825                 of_property_read_string(node, "line_length", &str);
 826                 if (str == NULL) {
 827                         dev_err(&client->dev, "Failed to read mode line_length\n");
 828                         return -ENODATA;
 829                 };
 830                 err = kstrtoint(str, 10, &pdata->mode_info[i].line_length);
 831                 if (err) {
 832                         dev_err(&client->dev, "Failed to convert mode line_length\n");
 833                         return -EFAULT;
 834                 }
 835
 836                 of_property_read_string(node, "pix_clk_hz", &str);
 837                 if (str == NULL) {
 838                         dev_err(&client->dev, "Failed to read mode pix_clk_hz\n");
 839                         return -ENODATA;
 840                 };
 841                 err = kstrtoll(str, 10, &pdata->mode_info[i].pixel_clock);
 842                 if (err) {
 843                         dev_err(&client->dev, "Failed to convert mode pix_clk_hz\n");
 844                         return -EFAULT;
 845                 }
 846                 dev_info(&client->dev, "%s: line_length = %d, pixel_clock = %llu\n",
 847                         __func__, pdata->mode_info[i].line_length,
 848                         pdata->mode_info[i].pixel_clock);
 849         }
 850
 851         return 0;
 852 }
 853 EXPORT_SYMBOL(camera_common_parse_sensor_mode);