drivers/media/platform/tegra/csi/csi.c

   1 /*
   2  * NVIDIA Tegra CSI Device
   3  *
   4  * Copyright (c) 2015-2016, NVIDIA CORPORATION.  All rights reserved.
   5  *
   6  * Author: Bryan Wu <pengw@nvidia.com>
   7  *
   8  * This program is free software; you can redistribute it and/or modify
   9  * it under the terms of the GNU General Public License version 2 as
  10  * published by the Free Software Foundation.
  11  */
  12
  13 #include <linux/clk.h>
  14 #include <linux/device.h>
  15 #include <linux/gpio/consumer.h>
  16 #include <linux/module.h>
  17 #include <linux/of.h>
  18 #include <linux/of_graph.h>
  19 #include <linux/platform_device.h>
  20
  21 #include <media/media-entity.h>
  22 #include <media/v4l2-async.h>
  23 #include <media/v4l2-ctrls.h>
  24 #include <media/camera_common.h>
  25
  26 #include "dev.h"
  27 #include "vi/vi.h"
  28 #include "csi/csi.h"
  29
  30 static int csi_get_clks(struct tegra_csi_device *csi,
  31                         struct platform_device *pdev)
  32 {
  33         csi->clk = devm_clk_get(&pdev->dev, "csi");
  34         if (IS_ERR(csi->clk)) {
  35                 dev_err(&pdev->dev, "Failed to get csi clock\n");
  36                 return PTR_ERR(csi->clk);
  37         }
  38
  39         csi->tpg_clk = devm_clk_get(&pdev->dev, "pll_d");
  40         if (IS_ERR(csi->tpg_clk)) {
  41                 dev_err(&pdev->dev, "Failed to get tpg clock\n");
  42                 return PTR_ERR(csi->tpg_clk);
  43         }
  44
  45         csi->cil[0] = devm_clk_get(&pdev->dev, "cilab");
  46         if (IS_ERR(csi->cil[0])) {
  47                 dev_err(&pdev->dev, "Failed to get cilab clock\n");
  48                 return PTR_ERR(csi->cil[0]);
  49         }
  50
  51         csi->cil[1] = devm_clk_get(&pdev->dev, "cilcd");
  52         if (IS_ERR(csi->cil[1])) {
  53                 dev_err(&pdev->dev, "Failed to get cilcd clock\n");
  54                 return PTR_ERR(csi->cil[1]);
  55         }
  56
  57         csi->cil[2] = devm_clk_get(&pdev->dev, "cile");
  58         if (IS_ERR(csi->cil[2])) {
  59                 dev_err(&pdev->dev, "Failed to get cile clock\n");
  60                 return PTR_ERR(csi->cil[2]);
  61         }
  62
  63         return 0;
  64 }
  65
  66 static int set_csi_properties(struct tegra_csi_device *csi,
  67                         struct platform_device *pdev)
  68 {
  69         struct camera_common_data *s_data = &csi->s_data[0];
  70
  71         /*
  72         * These values are only used for tpg mode
  73         * With sensor, CSI power and clock info are provided
  74         * by the sensor sub device
  75         */
  76         s_data->csi_port = 0;
  77         s_data->numlanes = 12;
  78         csi->clk_freq = TEGRA_CLOCK_CSI_PORT_MAX;
  79
  80         if (!csi->ports) {
  81                 int port_num = (s_data->numlanes >> 1);
  82                 csi->ports = devm_kzalloc(&pdev->dev,
  83                         (port_num * sizeof(struct tegra_csi_port)),
  84                         GFP_KERNEL);
  85                 if (!csi->ports)
  86                         return -ENOMEM;
  87                 csi->num_ports = port_num;
  88         }
  89
  90         return 0;
  91 }
  92
  93 void set_csi_portinfo(struct tegra_csi_device *csi,
  94         unsigned int port, unsigned int numlanes)
  95 {
  96         struct camera_common_data *s_data = &csi->s_data[port];
  97
  98         s_data->csi_port = port;
  99         s_data->numlanes = numlanes;
 100         s_data->def_clk_freq = TEGRA_CLOCK_CSI_PORT_MAX;
 101         csi->ports[port].lanes = numlanes;
 102 }
 103 EXPORT_SYMBOL(set_csi_portinfo);
 104
 105 static void set_csi_registers(struct tegra_csi_device *csi,
 106                 void __iomem *regbase)
 107 {
 108         int i, j, idx;
 109
 110         csi->iomem[0] = (regbase + TEGRA_CSI_PIXEL_PARSER_0_BASE);
 111         csi->iomem[1] = (regbase + TEGRA_CSI_PIXEL_PARSER_2_BASE);
 112         csi->iomem[2] = (regbase + TEGRA_CSI_PIXEL_PARSER_4_BASE);
 113
 114         for (j = 0; j < 3; j++) {
 115                 for (i = 0; i < 2; i++) {
 116                         idx = (j << 1) + i;
 117                         /* Initialize port register bases */
 118                         csi->ports[idx].pixel_parser = csi->iomem[j] +
 119                                 i * TEGRA_CSI_PORT_OFFSET;
 120                         csi->ports[idx].cil = csi->iomem[j] +
 121                                 TEGRA_CSI_CIL_OFFSET +
 122                                 i * TEGRA_CSI_PORT_OFFSET;
 123                         csi->ports[idx].tpg = csi->iomem[j] +
 124                                 TEGRA_CSI_TPG_OFFSET +
 125                                 i * TEGRA_CSI_PORT_OFFSET;
 126
 127                         csi->ports[idx].num = idx;
 128                         csi->ports[idx].lanes = 2;
 129                 }
 130         }
 131 }
 132
 133 static int clock_start(struct tegra_csi_device *csi,
 134                        struct clk *clk, unsigned int freq)
 135 {
 136         int err = 0;
 137
 138         err = clk_prepare_enable(clk);
 139         if (err)
 140                 dev_err(csi->dev, "csi clk enable error %d\n", err);
 141         err = clk_set_rate(clk, freq);
 142         if (err)
 143                 dev_err(csi->dev, "csi clk set rate error %d\n", err);
 144
 145         return err;
 146 }
 147
 148 void tegra_csi_pad_control(struct tegra_csi_device *csi,
 149                                 unsigned char *port_num, int enable)
 150 {
 151         int i, port;
 152
 153         if (enable) {
 154                 for (i = 0; csi_port_is_valid(port_num[i]); i++) {
 155                         port = port_num[i];
 156                         camera_common_dpd_disable(&csi->s_data[port]);
 157                 }
 158         } else {
 159                 for (i = 0; csi_port_is_valid(port_num[i]); i++) {
 160                         port = port_num[i];
 161                         camera_common_dpd_enable(&csi->s_data[port]);
 162                 }
 163         }
 164 }
 165 EXPORT_SYMBOL(tegra_csi_pad_control);
 166
 167 int tegra_csi_channel_power(struct tegra_csi_device *csi,
 168                                 unsigned char *port_num, int enable)
 169 {
 170         int err = 0;
 171         int i, cil_num, port;
 172
 173         if (enable) {
 174                 for (i = 0; csi_port_is_valid(port_num[i]); i++) {
 175                         port = port_num[i];
 176                         cil_num = port >> 1;
 177                         err = clock_start(csi,
 178                                 csi->cil[cil_num], csi->clk_freq);
 179                         if (err)
 180                                 dev_err(csi->dev, "cil clk start error\n");
 181                         camera_common_dpd_disable(&csi->s_data[port]);
 182                 }
 183         } else {
 184                 for (i = 0; csi_port_is_valid(port_num[i]); i++) {
 185                         port = port_num[i];
 186                         cil_num = port >> 1;
 187                         camera_common_dpd_enable(&csi->s_data[port]);
 188                         clk_disable_unprepare(csi->cil[cil_num]);
 189                 }
 190         }
 191
 192         return err;
 193 }
 194 EXPORT_SYMBOL(tegra_csi_channel_power);
 195
 196 int tegra_csi_power(struct tegra_csi_device *csi, int enable)
 197 {
 198         int err = 0;
 199
 200         if (enable) {
 201                 /* set clk and power */
 202                 err = clk_prepare_enable(csi->clk);
 203                 if (err)
 204                         dev_err(csi->dev, "csi clk enable error\n");
 205
 206                 if (csi->pg_mode) {
 207                         err = clock_start(csi, csi->tpg_clk,
 208                                                 TEGRA_CLOCK_TPG_MAX);
 209                         if (err)
 210                                 dev_err(csi->dev, "tpg clk start error\n");
 211                         else {
 212                                 tegra_clk_cfg_ex(csi->tpg_clk,
 213                                         TEGRA_CLK_PLLD_CSI_OUT_ENB, 1);
 214                                 tegra_clk_cfg_ex(csi->tpg_clk,
 215                                         TEGRA_CLK_PLLD_DSI_OUT_ENB, 1);
 216                                 tegra_clk_cfg_ex(csi->tpg_clk,
 217                                         TEGRA_CLK_MIPI_CSI_OUT_ENB, 0);
 218                         }
 219                 }
 220         } else {
 221                 if (csi->pg_mode) {
 222                         tegra_clk_cfg_ex(csi->tpg_clk,
 223                                          TEGRA_CLK_MIPI_CSI_OUT_ENB, 1);
 224                         tegra_clk_cfg_ex(csi->tpg_clk,
 225                                          TEGRA_CLK_PLLD_CSI_OUT_ENB, 0);
 226                         tegra_clk_cfg_ex(csi->tpg_clk,
 227                                          TEGRA_CLK_PLLD_DSI_OUT_ENB, 0);
 228                         clk_disable_unprepare(csi->tpg_clk);
 229                 }
 230                 clk_disable_unprepare(csi->clk);
 231         }
 232
 233         return err;
 234 }
 235 EXPORT_SYMBOL(tegra_csi_power);
 236
 237 /*
 238  * -----------------------------------------------------------------------------
 239  * CSI Subdevice Video Operations
 240  * -----------------------------------------------------------------------------
 241  */
 242
 243 /* Test Pattern Generator setup */
 244 void tegra_csi_tpg_start_streaming(struct tegra_csi_device *csi,
 245                                    enum tegra_csi_port_num port_num)
 246 {
 247         csi->fops->soc_tpg_start_streaming(csi, port_num);
 248 }
 249
 250 void tegra_csi_start_streaming(struct tegra_csi_device *csi,
 251                                 enum tegra_csi_port_num port_num)
 252 {
 253         csi->fops->soc_start_streaming(csi, port_num);
 254 }
 255 EXPORT_SYMBOL(tegra_csi_start_streaming);
 256
 257 int tegra_csi_error(struct tegra_csi_device *csi,
 258                         enum tegra_csi_port_num port_num)
 259 {
 260         return csi->fops->soc_error(csi, port_num);
 261 }
 262
 263 void tegra_csi_status(struct tegra_csi_device *csi,
 264                         enum tegra_csi_port_num port_num)
 265 {
 266         csi->fops->soc_status(csi, port_num);
 267 }
 268 EXPORT_SYMBOL(tegra_csi_status);
 269
 270 void tegra_csi_error_recover(struct tegra_csi_device *csi,
 271                                 enum tegra_csi_port_num port_num)
 272 {
 273         csi->fops->soc_error_recover(csi, port_num);
 274 }
 275
 276 void tegra_csi_stop_streaming(struct tegra_csi_device *csi,
 277                                 enum tegra_csi_port_num port_num)
 278 {
 279         csi->fops->soc_stop_streaming(csi, port_num);
 280 }
 281 EXPORT_SYMBOL(tegra_csi_stop_streaming);
 282
 283 static int tegra_csi_s_stream(struct v4l2_subdev *subdev, int enable)
 284 {
 285         struct tegra_csi_device *csi = to_csi(subdev);
 286         struct tegra_channel *chan;
 287         int index;
 288
 289         if (csi->pg_mode)
 290                 return 0;
 291
 292         chan = subdev->host_priv;
 293         for (index = 0; index < chan->valid_ports; index++) {
 294                 enum tegra_csi_port_num port_num = chan->port[index];
 295                 if (enable)
 296                         tegra_csi_start_streaming(csi, port_num);
 297                 else
 298                         tegra_csi_stop_streaming(csi, port_num);
 299         }
 300
 301         return 0;
 302 }
 303
 304 /*
 305  * Only use this subdevice media bus ops for test pattern generator,
 306  * because CSI device is an separated subdevice which has 6 source
 307  * pads to generate test pattern.
 308  */
 309 static struct v4l2_mbus_framefmt tegra_csi_tpg_fmts[] = {
 310         {
 311                 TEGRA_DEF_WIDTH,
 312                 TEGRA_DEF_HEIGHT,
 313                 V4L2_MBUS_FMT_SRGGB10_1X10,
 314                 V4L2_FIELD_NONE,
 315                 V4L2_COLORSPACE_SRGB
 316         },
 317         {
 318                 TEGRA_DEF_WIDTH,
 319                 TEGRA_DEF_HEIGHT,
 320                 V4L2_MBUS_FMT_RGBA8888_4X8_LE,
 321                 V4L2_FIELD_NONE,
 322                 V4L2_COLORSPACE_SRGB
 323         }
 324
 325 };
 326
 327 static struct v4l2_frmsize_discrete tegra_csi_tpg_sizes[] = {
 328         {1280, 720},
 329         {1920, 1080},
 330         {3840, 2160}
 331 };
 332
 333 static int tegra_csi_enum_framesizes(struct v4l2_subdev *sd,
 334                                      struct v4l2_frmsizeenum *sizes)
 335 {
 336         int i;
 337         struct tegra_csi_device *csi = to_csi(sd);
 338
 339         if (!csi->pg_mode) {
 340                 dev_err(csi->dev, "CSI is not in TPG mode\n");
 341                 return -EINVAL;
 342         }
 343
 344         if (sizes->index >= ARRAY_SIZE(tegra_csi_tpg_sizes))
 345                 return -EINVAL;
 346
 347         for (i = 0; i < ARRAY_SIZE(tegra_csi_tpg_fmts); i++) {
 348                 const struct tegra_video_format *format =
 349                       tegra_core_get_format_by_code(tegra_csi_tpg_fmts[i].code);
 350                 if (format && format->fourcc == sizes->pixel_format)
 351                         break;
 352         }
 353         if (i == ARRAY_SIZE(tegra_csi_tpg_fmts))
 354                 return -EINVAL;
 355
 356         sizes->type = V4L2_FRMSIZE_TYPE_DISCRETE;
 357         sizes->discrete = tegra_csi_tpg_sizes[sizes->index];
 358         return 0;
 359 }
 360
 361 #define TPG_PIXEL_OUTPUT_RATE 182476800
 362
 363 static int tegra_csi_enum_frameintervals(struct v4l2_subdev *sd,
 364                                      struct v4l2_frmivalenum *intervals)
 365 {
 366         int i;
 367         struct tegra_csi_device *csi = to_csi(sd);
 368
 369         if (!csi->pg_mode) {
 370                 dev_err(csi->dev, "CSI is not in TPG mode\n");
 371                 return -EINVAL;
 372         }
 373
 374         /* One resolution just one framerate */
 375         if (intervals->index > 0)
 376                 return -EINVAL;
 377
 378         for (i = 0; i < ARRAY_SIZE(tegra_csi_tpg_fmts); i++) {
 379                 const struct tegra_video_format *format =
 380                       tegra_core_get_format_by_code(tegra_csi_tpg_fmts[i].code);
 381                 if (format && format->fourcc == intervals->pixel_format)
 382                         break;
 383         }
 384         if (i == ARRAY_SIZE(tegra_csi_tpg_fmts))
 385                 return -EINVAL;
 386
 387         for (i = 0; i < ARRAY_SIZE(tegra_csi_tpg_sizes); i++) {
 388                 if (tegra_csi_tpg_sizes[i].width == intervals->width &&
 389                     tegra_csi_tpg_sizes[i].height == intervals->height)
 390                         break;
 391         }
 392         if (i == ARRAY_SIZE(tegra_csi_tpg_sizes))
 393                 return -EINVAL;
 394
 395         intervals->type = V4L2_FRMIVAL_TYPE_DISCRETE;
 396         intervals->discrete.numerator = 1;
 397         intervals->discrete.denominator = TPG_PIXEL_OUTPUT_RATE /
 398                    (intervals->width * intervals->height);
 399         return 0;
 400 }
 401
 402 static int tegra_csi_try_mbus_fmt(struct v4l2_subdev *sd,
 403                                   struct v4l2_mbus_framefmt *mf)
 404 {
 405         int i, j;
 406         struct tegra_csi_device *csi = to_csi(sd);
 407         static struct v4l2_frmsize_discrete *sizes;
 408
 409         if (!csi->pg_mode) {
 410                 dev_err(csi->dev, "CSI is not in TPG mode\n");
 411                 return -EINVAL;
 412         }
 413
 414         for (i = 0; i < ARRAY_SIZE(tegra_csi_tpg_fmts); i++) {
 415                 struct v4l2_mbus_framefmt *fmt = &tegra_csi_tpg_fmts[i];
 416                 if (mf->code == fmt->code && mf->field == fmt->field &&
 417                     mf->colorspace == fmt->colorspace) {
 418                         for (j = 0; j < ARRAY_SIZE(tegra_csi_tpg_sizes); j++) {
 419                                 sizes = &tegra_csi_tpg_sizes[j];
 420                                 if (mf->width == sizes->width &&
 421                                     mf->height == sizes->height)
 422                                         return 0;
 423                         }
 424                 }
 425         }
 426
 427         memcpy(mf, tegra_csi_tpg_fmts, sizeof(struct v4l2_mbus_framefmt));
 428
 429         return 0;
 430 }
 431
 432 static int tegra_csi_s_mbus_fmt(struct v4l2_subdev *sd,
 433                                 struct v4l2_mbus_framefmt *fmt)
 434 {
 435         int i;
 436         struct tegra_csi_device *csi = to_csi(sd);
 437
 438         if (!csi->pg_mode) {
 439                 dev_err(csi->dev, "CSI is not in TPG mode\n");
 440                 return -EINVAL;
 441         }
 442
 443         tegra_csi_try_mbus_fmt(sd, fmt);
 444
 445         for (i = 0; i < csi->num_ports; i++) {
 446                 struct v4l2_mbus_framefmt *format = &csi->ports[i].format;
 447                 memcpy(format, fmt, sizeof(struct v4l2_mbus_framefmt));
 448         }
 449
 450         return 0;
 451 }
 452
 453 static int tegra_csi_g_mbus_fmt(struct v4l2_subdev *sd,
 454                                 struct v4l2_mbus_framefmt *fmt)
 455 {
 456         return tegra_csi_try_mbus_fmt(sd, fmt);
 457 }
 458
 459 static int tegra_csi_g_input_status(struct v4l2_subdev *sd, u32 *status)
 460 {
 461         struct tegra_csi_device *csi = to_csi(sd);
 462
 463         *status = !!csi->pg_mode;
 464
 465         return 0;
 466 }
 467
 468 /* -----------------------------------------------------------------------------
 469  * V4L2 Subdevice Pad Operations
 470  */
 471
 472 static int tegra_csi_get_format(struct v4l2_subdev *subdev,
 473                            struct v4l2_subdev_fh *cfg,
 474                            struct v4l2_subdev_format *fmt)
 475 {
 476         struct v4l2_mbus_framefmt mbus_fmt;
 477         int ret;
 478
 479         ret = tegra_csi_g_mbus_fmt(subdev, &mbus_fmt);
 480         if (ret)
 481                 return ret;
 482
 483         fmt->format = mbus_fmt;
 484
 485         return 0;
 486 }
 487
 488 static int tegra_csi_set_format(struct v4l2_subdev *subdev,
 489                            struct v4l2_subdev_fh *cfg,
 490                            struct v4l2_subdev_format *fmt)
 491 {
 492         int i, ret;
 493         struct tegra_csi_device *csi = to_csi(subdev);
 494
 495         ret = tegra_csi_try_mbus_fmt(subdev, &fmt->format);
 496         if (ret)
 497                 return ret;
 498
 499         if (fmt->which == V4L2_SUBDEV_FORMAT_TRY)
 500                 return 0;
 501
 502         for (i = 0; i < csi->num_ports; i++) {
 503                 struct v4l2_mbus_framefmt *format = &csi->ports[i].format;
 504                 memcpy(format, &fmt->format, sizeof(struct v4l2_mbus_framefmt));
 505         }
 506
 507         return 0;
 508 }
 509
 510 /* -----------------------------------------------------------------------------
 511  * V4L2 Subdevice Operations
 512  */
 513 static struct v4l2_subdev_video_ops tegra_csi_video_ops = {
 514         .s_stream       = tegra_csi_s_stream,
 515         .try_mbus_fmt   = tegra_csi_try_mbus_fmt,
 516         .s_mbus_fmt     = tegra_csi_s_mbus_fmt,
 517         .g_mbus_fmt     = tegra_csi_g_mbus_fmt,
 518         .g_input_status = tegra_csi_g_input_status,
 519         .enum_framesizes = tegra_csi_enum_framesizes,
 520         .enum_frameintervals = tegra_csi_enum_frameintervals,
 521 };
 522
 523 static struct v4l2_subdev_pad_ops tegra_csi_pad_ops = {
 524         .get_fmt        = tegra_csi_get_format,
 525         .set_fmt        = tegra_csi_set_format,
 526 };
 527
 528 static struct v4l2_subdev_ops tegra_csi_ops = {
 529         .video  = &tegra_csi_video_ops,
 530         .pad    = &tegra_csi_pad_ops,
 531 };
 532
 533 /* -----------------------------------------------------------------------------
 534  * Media Operations
 535  */
 536
 537 static const struct media_entity_operations tegra_csi_media_ops = {
 538         .link_validate = v4l2_subdev_link_validate,
 539 };
 540
 541 /* -----------------------------------------------------------------------------
 542  * Platform Device Driver
 543  */
 544
 545 static int tegra_csi_parse_of(struct tegra_csi_device *csi,
 546                         struct platform_device *pdev)
 547 {
 548         struct device_node *node = csi->dev->of_node;
 549         struct device_node *ports;
 550         struct device_node *port;
 551         struct device_node *ep;
 552         unsigned int lanes, pad_num, port_num;
 553         int ret;
 554
 555         ret = of_property_read_u32(node, "num-ports", &port_num);
 556         if (ret < 0)
 557                 return ret;
 558
 559         csi->ports = devm_kzalloc(&pdev->dev,
 560                 (port_num * sizeof(struct tegra_csi_port)), GFP_KERNEL);
 561         if (!csi->ports)
 562                 return -ENOMEM;
 563         csi->num_ports = port_num;
 564
 565         csi->pads = devm_kzalloc(&pdev->dev,
 566                 (port_num * 2 * sizeof(struct media_pad)), GFP_KERNEL);
 567         if (!csi->pads)
 568                 return -ENOMEM;
 569
 570         ports = of_get_child_by_name(node, "ports");
 571         if (ports == NULL)
 572                 ports = node;
 573
 574         for_each_child_of_node(ports, port) {
 575                 if (!port->name || of_node_cmp(port->name, "port"))
 576                         continue;
 577
 578                 ret = of_property_read_u32(port, "reg", &pad_num);
 579                 if (ret < 0)
 580                         continue;
 581                 port_num = (pad_num >> 1);
 582                 csi->ports[port_num].num = port_num;
 583
 584                 for_each_child_of_node(port, ep) {
 585                         if (!ep->name || of_node_cmp(ep->name, "endpoint"))
 586                                 continue;
 587
 588                         /* Get number of data lanes for the first endpoint */
 589                         ret = of_property_read_u32(ep, "bus-width", &lanes);
 590                         if (ret < 0)
 591                                 lanes = 4;
 592                         csi->ports[port_num].lanes = lanes;
 593                 }
 594         }
 595
 596         return 0;
 597 }
 598
 599 static int tegra_tpg_csi_parse_data(struct tegra_csi_device *csi,
 600                                     struct platform_device *pdev)
 601 {
 602         int i;
 603
 604         csi->ports = devm_kzalloc(&pdev->dev,
 605                 (csi->num_ports * sizeof(struct tegra_csi_port)), GFP_KERNEL);
 606         if (!csi->ports)
 607                 return -ENOMEM;
 608
 609         csi->pads = devm_kzalloc(&pdev->dev,
 610                 (csi->num_ports * sizeof(struct media_pad)), GFP_KERNEL);
 611         if (!csi->pads)
 612                 return -ENOMEM;
 613
 614         for (i = 0; i < csi->num_ports; i++) {
 615                 csi->ports[i].num = i;
 616                 csi->ports[i].lanes = 2;
 617         }
 618
 619         return 0;
 620 }
 621
 622 int tegra_csi_init(struct tegra_csi_device *csi,
 623                 struct platform_device *pdev)
 624 {
 625         int err = 0;
 626         struct nvhost_device_data *pdata = pdev->dev.platform_data;
 627
 628         csi->dev = &pdev->dev;
 629         err = set_csi_properties(csi, pdev);
 630         if (err)
 631                 return err;
 632
 633         set_csi_registers(csi, pdata->aperture[0]);
 634
 635         err = csi_get_clks(csi, pdev);
 636         if (err)
 637                 dev_err(&pdev->dev, "Failed to get CSI clks\n");
 638
 639         return err;
 640 }
 641 EXPORT_SYMBOL(tegra_csi_init);
 642
 643 int tegra_csi_media_controller_init(struct tegra_csi_device *csi,
 644                                     struct platform_device *pdev)
 645 {
 646         struct v4l2_subdev *subdev;
 647         int ret, i;
 648
 649         csi->dev = &pdev->dev;
 650
 651         if (csi->pg_mode)
 652                 ret = tegra_tpg_csi_parse_data(csi, pdev);
 653         else
 654                 ret = tegra_csi_parse_of(csi, pdev);
 655         if (ret < 0)
 656                 return ret;
 657
 658         ret = tegra_csi_init(csi, pdev);
 659         if (ret < 0)
 660                 return ret;
 661
 662         /* Initialize V4L2 subdevice and media entity */
 663         subdev = &csi->subdev;
 664         v4l2_subdev_init(subdev, &tegra_csi_ops);
 665         subdev->dev = &pdev->dev;
 666         strlcpy(subdev->name, dev_name(&pdev->dev), sizeof(subdev->name));
 667         v4l2_set_subdevdata(subdev, csi);
 668         subdev->flags |= V4L2_SUBDEV_FL_HAS_DEVNODE;
 669         subdev->entity.ops = &tegra_csi_media_ops;
 670
 671         for (i = 0; i < csi->num_ports; i++) {
 672                 /* Initialize the default format */
 673                 csi->ports[i].format.code = TEGRA_VF_DEF;
 674                 csi->ports[i].format.field = V4L2_FIELD_NONE;
 675                 csi->ports[i].format.colorspace = V4L2_COLORSPACE_SRGB;
 676                 csi->ports[i].format.width = TEGRA_DEF_WIDTH;
 677                 csi->ports[i].format.height = TEGRA_DEF_HEIGHT;
 678
 679                 if (csi->pg_mode)
 680                         csi->pads[i].flags = MEDIA_PAD_FL_SOURCE;
 681                 else {
 682                         csi->pads[i * 2].flags = MEDIA_PAD_FL_SINK;
 683                         csi->pads[i * 2 + 1].flags = MEDIA_PAD_FL_SOURCE;
 684                 }
 685         }
 686
 687         /* Initialize media entity */
 688         ret = media_entity_init(&subdev->entity,
 689                                 csi->pg_mode ? csi->num_ports :
 690                                 csi->num_ports * 2,
 691                                 csi->pads, 0);
 692         if (ret < 0)
 693                 return ret;
 694
 695         ret = v4l2_async_register_subdev(subdev);
 696         if (ret < 0) {
 697                 dev_err(&pdev->dev, "failed to register subdev\n");
 698                 goto error;
 699         }
 700
 701         return 0;
 702
 703 error:
 704         media_entity_cleanup(&subdev->entity);
 705         return ret;
 706 }
 707 EXPORT_SYMBOL(tegra_csi_media_controller_init);
 708
 709 int tegra_csi_media_controller_remove(struct tegra_csi_device *csi)
 710 {
 711         struct v4l2_subdev *subdev = &csi->subdev;
 712
 713         v4l2_async_unregister_subdev(subdev);
 714         media_entity_cleanup(&subdev->entity);
 715         return 0;
 716 }
 717 EXPORT_SYMBOL(tegra_csi_media_controller_remove);