drivers/staging/media/tegra-video/csi.c

   1 // SPDX-License-Identifier: GPL-2.0-only
   2 /*
   3  * Copyright (C) 2020 NVIDIA CORPORATION.  All rights reserved.
   4  */
   5
   6 #include <linux/clk.h>
   7 #include <linux/clk/tegra.h>
   8 #include <linux/device.h>
   9 #include <linux/host1x.h>
  10 #include <linux/module.h>
  11 #include <linux/of.h>
  12 #include <linux/of_graph.h>
  13 #include <linux/of_device.h>
  14 #include <linux/platform_device.h>
  15 #include <linux/pm_runtime.h>
  16
  17 #include <media/v4l2-fwnode.h>
  18
  19 #include "csi.h"
  20 #include "video.h"
  21
  22 #define MHZ                     1000000
  23
  24 static inline struct tegra_csi *
  25 host1x_client_to_csi(struct host1x_client *client)
  26 {
  27         return container_of(client, struct tegra_csi, client);
  28 }
  29
  30 static inline struct tegra_csi_channel *to_csi_chan(struct v4l2_subdev *subdev)
  31 {
  32         return container_of(subdev, struct tegra_csi_channel, subdev);
  33 }
  34
  35 /*
  36  * CSI is a separate subdevice which has 6 source pads to generate
  37  * test pattern. CSI subdevice pad ops are used only for TPG and
  38  * allows below TPG formats.
  39  */
  40 static const struct v4l2_mbus_framefmt tegra_csi_tpg_fmts[] = {
  41         {
  42                 TEGRA_DEF_WIDTH,
  43                 TEGRA_DEF_HEIGHT,
  44                 MEDIA_BUS_FMT_SRGGB10_1X10,
  45                 V4L2_FIELD_NONE,
  46                 V4L2_COLORSPACE_SRGB
  47         },
  48         {
  49                 TEGRA_DEF_WIDTH,
  50                 TEGRA_DEF_HEIGHT,
  51                 MEDIA_BUS_FMT_RGB888_1X32_PADHI,
  52                 V4L2_FIELD_NONE,
  53                 V4L2_COLORSPACE_SRGB
  54         },
  55 };
  56
  57 static const struct v4l2_frmsize_discrete tegra_csi_tpg_sizes[] = {
  58         { 1280, 720 },
  59         { 1920, 1080 },
  60         { 3840, 2160 },
  61 };
  62
  63 /*
  64  * V4L2 Subdevice Pad Operations
  65  */
  66 static int csi_enum_bus_code(struct v4l2_subdev *subdev,
  67                              struct v4l2_subdev_state *sd_state,
  68                              struct v4l2_subdev_mbus_code_enum *code)
  69 {
  70         if (!IS_ENABLED(CONFIG_VIDEO_TEGRA_TPG))
  71                 return -ENOIOCTLCMD;
  72
  73         if (code->index >= ARRAY_SIZE(tegra_csi_tpg_fmts))
  74                 return -EINVAL;
  75
  76         code->code = tegra_csi_tpg_fmts[code->index].code;
  77
  78         return 0;
  79 }
  80
  81 static int csi_get_format(struct v4l2_subdev *subdev,
  82                           struct v4l2_subdev_state *sd_state,
  83                           struct v4l2_subdev_format *fmt)
  84 {
  85         struct tegra_csi_channel *csi_chan = to_csi_chan(subdev);
  86
  87         if (!IS_ENABLED(CONFIG_VIDEO_TEGRA_TPG))
  88                 return -ENOIOCTLCMD;
  89
  90         fmt->format = csi_chan->format;
  91
  92         return 0;
  93 }
  94
  95 static int csi_get_frmrate_table_index(struct tegra_csi *csi, u32 code,
  96                                        u32 width, u32 height)
  97 {
  98         const struct tpg_framerate *frmrate;
  99         unsigned int i;
 100
 101         frmrate = csi->soc->tpg_frmrate_table;
 102         for (i = 0; i < csi->soc->tpg_frmrate_table_size; i++) {
 103                 if (frmrate[i].code == code &&
 104                     frmrate[i].frmsize.width == width &&
 105                     frmrate[i].frmsize.height == height) {
 106                         return i;
 107                 }
 108         }
 109
 110         return -EINVAL;
 111 }
 112
 113 static void csi_chan_update_blank_intervals(struct tegra_csi_channel *csi_chan,
 114                                             u32 code, u32 width, u32 height)
 115 {
 116         struct tegra_csi *csi = csi_chan->csi;
 117         const struct tpg_framerate *frmrate = csi->soc->tpg_frmrate_table;
 118         int index;
 119
 120         index = csi_get_frmrate_table_index(csi_chan->csi, code,
 121                                             width, height);
 122         if (index >= 0) {
 123                 csi_chan->h_blank = frmrate[index].h_blank;
 124                 csi_chan->v_blank = frmrate[index].v_blank;
 125                 csi_chan->framerate = frmrate[index].framerate;
 126         }
 127 }
 128
 129 static int csi_enum_framesizes(struct v4l2_subdev *subdev,
 130                                struct v4l2_subdev_state *sd_state,
 131                                struct v4l2_subdev_frame_size_enum *fse)
 132 {
 133         unsigned int i;
 134
 135         if (!IS_ENABLED(CONFIG_VIDEO_TEGRA_TPG))
 136                 return -ENOIOCTLCMD;
 137
 138         if (fse->index >= ARRAY_SIZE(tegra_csi_tpg_sizes))
 139                 return -EINVAL;
 140
 141         for (i = 0; i < ARRAY_SIZE(tegra_csi_tpg_fmts); i++)
 142                 if (fse->code == tegra_csi_tpg_fmts[i].code)
 143                         break;
 144
 145         if (i == ARRAY_SIZE(tegra_csi_tpg_fmts))
 146                 return -EINVAL;
 147
 148         fse->min_width = tegra_csi_tpg_sizes[fse->index].width;
 149         fse->max_width = tegra_csi_tpg_sizes[fse->index].width;
 150         fse->min_height = tegra_csi_tpg_sizes[fse->index].height;
 151         fse->max_height = tegra_csi_tpg_sizes[fse->index].height;
 152
 153         return 0;
 154 }
 155
 156 static int csi_enum_frameintervals(struct v4l2_subdev *subdev,
 157                                    struct v4l2_subdev_state *sd_state,
 158                                    struct v4l2_subdev_frame_interval_enum *fie)
 159 {
 160         struct tegra_csi_channel *csi_chan = to_csi_chan(subdev);
 161         struct tegra_csi *csi = csi_chan->csi;
 162         const struct tpg_framerate *frmrate = csi->soc->tpg_frmrate_table;
 163         int index;
 164
 165         if (!IS_ENABLED(CONFIG_VIDEO_TEGRA_TPG))
 166                 return -ENOIOCTLCMD;
 167
 168         /* one framerate per format and resolution */
 169         if (fie->index > 0)
 170                 return -EINVAL;
 171
 172         index = csi_get_frmrate_table_index(csi_chan->csi, fie->code,
 173                                             fie->width, fie->height);
 174         if (index < 0)
 175                 return -EINVAL;
 176
 177         fie->interval.numerator = 1;
 178         fie->interval.denominator = frmrate[index].framerate;
 179
 180         return 0;
 181 }
 182
 183 static int csi_set_format(struct v4l2_subdev *subdev,
 184                           struct v4l2_subdev_state *sd_state,
 185                           struct v4l2_subdev_format *fmt)
 186 {
 187         struct tegra_csi_channel *csi_chan = to_csi_chan(subdev);
 188         struct v4l2_mbus_framefmt *format = &fmt->format;
 189         const struct v4l2_frmsize_discrete *sizes;
 190         unsigned int i;
 191
 192         if (!IS_ENABLED(CONFIG_VIDEO_TEGRA_TPG))
 193                 return -ENOIOCTLCMD;
 194
 195         sizes = v4l2_find_nearest_size(tegra_csi_tpg_sizes,
 196                                        ARRAY_SIZE(tegra_csi_tpg_sizes),
 197                                        width, height,
 198                                        format->width, format->width);
 199         format->width = sizes->width;
 200         format->height = sizes->height;
 201
 202         for (i = 0; i < ARRAY_SIZE(tegra_csi_tpg_fmts); i++)
 203                 if (format->code == tegra_csi_tpg_fmts[i].code)
 204                         break;
 205
 206         if (i == ARRAY_SIZE(tegra_csi_tpg_fmts))
 207                 i = 0;
 208
 209         format->code = tegra_csi_tpg_fmts[i].code;
 210         format->field = V4L2_FIELD_NONE;
 211
 212         if (fmt->which == V4L2_SUBDEV_FORMAT_TRY)
 213                 return 0;
 214
 215         /* update blanking intervals from frame rate table and format */
 216         csi_chan_update_blank_intervals(csi_chan, format->code,
 217                                         format->width, format->height);
 218         csi_chan->format = *format;
 219
 220         return 0;
 221 }
 222
 223 /*
 224  * V4L2 Subdevice Video Operations
 225  */
 226 static int tegra_csi_g_frame_interval(struct v4l2_subdev *subdev,
 227                                       struct v4l2_subdev_frame_interval *vfi)
 228 {
 229         struct tegra_csi_channel *csi_chan = to_csi_chan(subdev);
 230
 231         if (!IS_ENABLED(CONFIG_VIDEO_TEGRA_TPG))
 232                 return -ENOIOCTLCMD;
 233
 234         vfi->interval.numerator = 1;
 235         vfi->interval.denominator = csi_chan->framerate;
 236
 237         return 0;
 238 }
 239
 240 static unsigned int csi_get_pixel_rate(struct tegra_csi_channel *csi_chan)
 241 {
 242         struct tegra_vi_channel *chan;
 243         struct v4l2_subdev *src_subdev;
 244         struct v4l2_ctrl *ctrl;
 245
 246         chan = v4l2_get_subdev_hostdata(&csi_chan->subdev);
 247         src_subdev = tegra_channel_get_remote_source_subdev(chan);
 248         ctrl = v4l2_ctrl_find(src_subdev->ctrl_handler, V4L2_CID_PIXEL_RATE);
 249         if (ctrl)
 250                 return v4l2_ctrl_g_ctrl_int64(ctrl);
 251
 252         return 0;
 253 }
 254
 255 void tegra_csi_calc_settle_time(struct tegra_csi_channel *csi_chan,
 256                                 u8 csi_port_num,
 257                                 u8 *clk_settle_time,
 258                                 u8 *ths_settle_time)
 259 {
 260         struct tegra_csi *csi = csi_chan->csi;
 261         unsigned int cil_clk_mhz;
 262         unsigned int pix_clk_mhz;
 263         int clk_idx = (csi_port_num >> 1) + 1;
 264
 265         cil_clk_mhz = clk_get_rate(csi->clks[clk_idx].clk) / MHZ;
 266         pix_clk_mhz = csi_get_pixel_rate(csi_chan) / MHZ;
 267
 268         /*
 269          * CLK Settle time is the interval during which HS receiver should
 270          * ignore any clock lane HS transitions, starting from the beginning
 271          * of T-CLK-PREPARE.
 272          * Per DPHY specification, T-CLK-SETTLE should be between 95ns ~ 300ns
 273          *
 274          * 95ns < (clk-settle-programmed + 7) * lp clk period < 300ns
 275          * midpoint = 197.5 ns
 276          */
 277         *clk_settle_time = ((95 + 300) * cil_clk_mhz - 14000) / 2000;
 278
 279         /*
 280          * THS Settle time is the interval during which HS receiver should
 281          * ignore any data lane HS transitions, starting from the beginning
 282          * of THS-PREPARE.
 283          *
 284          * Per DPHY specification, T-HS-SETTLE should be between 85ns + 6UI
 285          * and 145ns+10UI.
 286          * 85ns + 6UI < (Ths-settle-prog + 5) * lp_clk_period < 145ns + 10UI
 287          * midpoint = 115ns + 8UI
 288          */
 289         if (pix_clk_mhz)
 290                 *ths_settle_time = (115 * cil_clk_mhz + 8000 * cil_clk_mhz
 291                                    / (2 * pix_clk_mhz) - 5000) / 1000;
 292 }
 293
 294 static int tegra_csi_enable_stream(struct v4l2_subdev *subdev)
 295 {
 296         struct tegra_vi_channel *chan = v4l2_get_subdev_hostdata(subdev);
 297         struct tegra_csi_channel *csi_chan = to_csi_chan(subdev);
 298         struct tegra_csi *csi = csi_chan->csi;
 299         int ret, err;
 300
 301         ret = pm_runtime_resume_and_get(csi->dev);
 302         if (ret < 0) {
 303                 dev_err(csi->dev, "failed to get runtime PM: %d\n", ret);
 304                 return ret;
 305         }
 306
 307         if (csi_chan->mipi) {
 308                 ret = tegra_mipi_enable(csi_chan->mipi);
 309                 if (ret < 0) {
 310                         dev_err(csi->dev,
 311                                 "failed to enable MIPI pads: %d\n", ret);
 312                         goto rpm_put;
 313                 }
 314
 315                 /*
 316                  * CSI MIPI pads PULLUP, PULLDN and TERM impedances need to
 317                  * be calibrated after power on.
 318                  * So, trigger the calibration start here and results will
 319                  * be latched and applied to the pads when link is in LP11
 320                  * state during start of sensor streaming.
 321                  */
 322                 ret = tegra_mipi_start_calibration(csi_chan->mipi);
 323                 if (ret < 0) {
 324                         dev_err(csi->dev,
 325                                 "failed to start MIPI calibration: %d\n", ret);
 326                         goto disable_mipi;
 327                 }
 328         }
 329
 330         csi_chan->pg_mode = chan->pg_mode;
 331         ret = csi->ops->csi_start_streaming(csi_chan);
 332         if (ret < 0)
 333                 goto finish_calibration;
 334
 335         return 0;
 336
 337 finish_calibration:
 338         if (csi_chan->mipi)
 339                 tegra_mipi_finish_calibration(csi_chan->mipi);
 340 disable_mipi:
 341         if (csi_chan->mipi) {
 342                 err = tegra_mipi_disable(csi_chan->mipi);
 343                 if (err < 0)
 344                         dev_err(csi->dev,
 345                                 "failed to disable MIPI pads: %d\n", err);
 346         }
 347
 348 rpm_put:
 349         pm_runtime_put(csi->dev);
 350         return ret;
 351 }
 352
 353 static int tegra_csi_disable_stream(struct v4l2_subdev *subdev)
 354 {
 355         struct tegra_csi_channel *csi_chan = to_csi_chan(subdev);
 356         struct tegra_csi *csi = csi_chan->csi;
 357         int err;
 358
 359         csi->ops->csi_stop_streaming(csi_chan);
 360
 361         if (csi_chan->mipi) {
 362                 err = tegra_mipi_disable(csi_chan->mipi);
 363                 if (err < 0)
 364                         dev_err(csi->dev,
 365                                 "failed to disable MIPI pads: %d\n", err);
 366         }
 367
 368         pm_runtime_put(csi->dev);
 369
 370         return 0;
 371 }
 372
 373 static int tegra_csi_s_stream(struct v4l2_subdev *subdev, int enable)
 374 {
 375         int ret;
 376
 377         if (enable)
 378                 ret = tegra_csi_enable_stream(subdev);
 379         else
 380                 ret = tegra_csi_disable_stream(subdev);
 381
 382         return ret;
 383 }
 384
 385 /*
 386  * V4L2 Subdevice Operations
 387  */
 388 static const struct v4l2_subdev_video_ops tegra_csi_video_ops = {
 389         .s_stream = tegra_csi_s_stream,
 390         .g_frame_interval = tegra_csi_g_frame_interval,
 391         .s_frame_interval = tegra_csi_g_frame_interval,
 392 };
 393
 394 static const struct v4l2_subdev_pad_ops tegra_csi_pad_ops = {
 395         .enum_mbus_code         = csi_enum_bus_code,
 396         .enum_frame_size        = csi_enum_framesizes,
 397         .enum_frame_interval    = csi_enum_frameintervals,
 398         .get_fmt                = csi_get_format,
 399         .set_fmt                = csi_set_format,
 400 };
 401
 402 static const struct v4l2_subdev_ops tegra_csi_ops = {
 403         .video  = &tegra_csi_video_ops,
 404         .pad    = &tegra_csi_pad_ops,
 405 };
 406
 407 static int tegra_csi_channel_alloc(struct tegra_csi *csi,
 408                                    struct device_node *node,
 409                                    unsigned int port_num, unsigned int lanes,
 410                                    unsigned int num_pads)
 411 {
 412         struct tegra_csi_channel *chan;
 413         int ret = 0, i;
 414
 415         chan = kzalloc(sizeof(*chan), GFP_KERNEL);
 416         if (!chan)
 417                 return -ENOMEM;
 418
 419         list_add_tail(&chan->list, &csi->csi_chans);
 420         chan->csi = csi;
 421         /*
 422          * Each CSI brick has maximum of 4 lanes.
 423          * For lanes more than 4, use multiple of immediate CSI bricks as gang.
 424          */
 425         if (lanes <= CSI_LANES_PER_BRICK) {
 426                 chan->numlanes = lanes;
 427                 chan->numgangports = 1;
 428         } else {
 429                 chan->numlanes = CSI_LANES_PER_BRICK;
 430                 chan->numgangports = lanes / CSI_LANES_PER_BRICK;
 431         }
 432
 433         for (i = 0; i < chan->numgangports; i++)
 434                 chan->csi_port_nums[i] = port_num + i * CSI_PORTS_PER_BRICK;
 435
 436         chan->of_node = node;
 437         chan->numpads = num_pads;
 438         if (num_pads & 0x2) {
 439                 chan->pads[0].flags = MEDIA_PAD_FL_SINK;
 440                 chan->pads[1].flags = MEDIA_PAD_FL_SOURCE;
 441         } else {
 442                 chan->pads[0].flags = MEDIA_PAD_FL_SOURCE;
 443         }
 444
 445         if (IS_ENABLED(CONFIG_VIDEO_TEGRA_TPG))
 446                 return 0;
 447
 448         chan->mipi = tegra_mipi_request(csi->dev, node);
 449         if (IS_ERR(chan->mipi)) {
 450                 ret = PTR_ERR(chan->mipi);
 451                 dev_err(csi->dev, "failed to get mipi device: %d\n", ret);
 452         }
 453
 454         return ret;
 455 }
 456
 457 static int tegra_csi_tpg_channels_alloc(struct tegra_csi *csi)
 458 {
 459         struct device_node *node = csi->dev->of_node;
 460         unsigned int port_num;
 461         unsigned int tpg_channels = csi->soc->csi_max_channels;
 462         int ret;
 463
 464         /* allocate CSI channel for each CSI x2 ports */
 465         for (port_num = 0; port_num < tpg_channels; port_num++) {
 466                 ret = tegra_csi_channel_alloc(csi, node, port_num, 2, 1);
 467                 if (ret < 0)
 468                         return ret;
 469         }
 470
 471         return 0;
 472 }
 473
 474 static int tegra_csi_channels_alloc(struct tegra_csi *csi)
 475 {
 476         struct device_node *node = csi->dev->of_node;
 477         struct v4l2_fwnode_endpoint v4l2_ep = {
 478                 .bus_type = V4L2_MBUS_CSI2_DPHY
 479         };
 480         struct fwnode_handle *fwh;
 481         struct device_node *channel;
 482         struct device_node *ep;
 483         unsigned int lanes, portno, num_pads;
 484         int ret;
 485
 486         for_each_child_of_node(node, channel) {
 487                 if (!of_node_name_eq(channel, "channel"))
 488                         continue;
 489
 490                 ret = of_property_read_u32(channel, "reg", &portno);
 491                 if (ret < 0)
 492                         continue;
 493
 494                 if (portno >= csi->soc->csi_max_channels) {
 495                         dev_err(csi->dev, "invalid port num %d for %pOF\n",
 496                                 portno, channel);
 497                         ret = -EINVAL;
 498                         goto err_node_put;
 499                 }
 500
 501                 ep = of_graph_get_endpoint_by_regs(channel, 0, 0);
 502                 if (!ep)
 503                         continue;
 504
 505                 fwh = of_fwnode_handle(ep);
 506                 ret = v4l2_fwnode_endpoint_parse(fwh, &v4l2_ep);
 507                 of_node_put(ep);
 508                 if (ret) {
 509                         dev_err(csi->dev,
 510                                 "failed to parse v4l2 endpoint for %pOF: %d\n",
 511                                 channel, ret);
 512                         goto err_node_put;
 513                 }
 514
 515                 lanes = v4l2_ep.bus.mipi_csi2.num_data_lanes;
 516                 /*
 517                  * Each CSI brick has maximum 4 data lanes.
 518                  * For lanes more than 4, validate lanes to be multiple of 4
 519                  * so multiple of consecutive CSI bricks can be ganged up for
 520                  * streaming.
 521                  */
 522                 if (!lanes || ((lanes & (lanes - 1)) != 0) ||
 523                     (lanes > CSI_LANES_PER_BRICK && ((portno & 1) != 0))) {
 524                         dev_err(csi->dev, "invalid data-lanes %d for %pOF\n",
 525                                 lanes, channel);
 526                         ret = -EINVAL;
 527                         goto err_node_put;
 528                 }
 529
 530                 num_pads = of_graph_get_endpoint_count(channel);
 531                 if (num_pads == TEGRA_CSI_PADS_NUM) {
 532                         ret = tegra_csi_channel_alloc(csi, channel, portno,
 533                                                       lanes, num_pads);
 534                         if (ret < 0)
 535                                 goto err_node_put;
 536                 }
 537         }
 538
 539         return 0;
 540
 541 err_node_put:
 542         of_node_put(channel);
 543         return ret;
 544 }
 545
 546 static int tegra_csi_channel_init(struct tegra_csi_channel *chan)
 547 {
 548         struct tegra_csi *csi = chan->csi;
 549         struct v4l2_subdev *subdev;
 550         int ret;
 551
 552         /* initialize the default format */
 553         chan->format.code = MEDIA_BUS_FMT_SRGGB10_1X10;
 554         chan->format.field = V4L2_FIELD_NONE;
 555         chan->format.colorspace = V4L2_COLORSPACE_SRGB;
 556         chan->format.width = TEGRA_DEF_WIDTH;
 557         chan->format.height = TEGRA_DEF_HEIGHT;
 558         csi_chan_update_blank_intervals(chan, chan->format.code,
 559                                         chan->format.width,
 560                                         chan->format.height);
 561         /* initialize V4L2 subdevice and media entity */
 562         subdev = &chan->subdev;
 563         v4l2_subdev_init(subdev, &tegra_csi_ops);
 564         subdev->dev = csi->dev;
 565         if (IS_ENABLED(CONFIG_VIDEO_TEGRA_TPG))
 566                 snprintf(subdev->name, V4L2_SUBDEV_NAME_SIZE, "%s-%d", "tpg",
 567                          chan->csi_port_nums[0]);
 568         else
 569                 snprintf(subdev->name, V4L2_SUBDEV_NAME_SIZE, "%s",
 570                          kbasename(chan->of_node->full_name));
 571
 572         v4l2_set_subdevdata(subdev, chan);
 573         subdev->fwnode = of_fwnode_handle(chan->of_node);
 574         subdev->entity.function = MEDIA_ENT_F_VID_IF_BRIDGE;
 575
 576         /* initialize media entity pads */
 577         ret = media_entity_pads_init(&subdev->entity, chan->numpads,
 578                                      chan->pads);
 579         if (ret < 0) {
 580                 dev_err(csi->dev,
 581                         "failed to initialize media entity: %d\n", ret);
 582                 subdev->dev = NULL;
 583                 return ret;
 584         }
 585
 586         if (!IS_ENABLED(CONFIG_VIDEO_TEGRA_TPG)) {
 587                 ret = v4l2_async_register_subdev(subdev);
 588                 if (ret < 0) {
 589                         dev_err(csi->dev,
 590                                 "failed to register subdev: %d\n", ret);
 591                         return ret;
 592                 }
 593         }
 594
 595         return 0;
 596 }
 597
 598 void tegra_csi_error_recover(struct v4l2_subdev *sd)
 599 {
 600         struct tegra_csi_channel *csi_chan = to_csi_chan(sd);
 601         struct tegra_csi *csi = csi_chan->csi;
 602
 603         /* stop streaming during error recovery */
 604         csi->ops->csi_stop_streaming(csi_chan);
 605         csi->ops->csi_err_recover(csi_chan);
 606         csi->ops->csi_start_streaming(csi_chan);
 607 }
 608
 609 static int tegra_csi_channels_init(struct tegra_csi *csi)
 610 {
 611         struct tegra_csi_channel *chan;
 612         int ret;
 613
 614         list_for_each_entry(chan, &csi->csi_chans, list) {
 615                 ret = tegra_csi_channel_init(chan);
 616                 if (ret) {
 617                         dev_err(csi->dev,
 618                                 "failed to initialize channel-%d: %d\n",
 619                                 chan->csi_port_nums[0], ret);
 620                         return ret;
 621                 }
 622         }
 623
 624         return 0;
 625 }
 626
 627 static void tegra_csi_channels_cleanup(struct tegra_csi *csi)
 628 {
 629         struct v4l2_subdev *subdev;
 630         struct tegra_csi_channel *chan, *tmp;
 631
 632         list_for_each_entry_safe(chan, tmp, &csi->csi_chans, list) {
 633                 if (chan->mipi)
 634                         tegra_mipi_free(chan->mipi);
 635
 636                 subdev = &chan->subdev;
 637                 if (subdev->dev) {
 638                         if (!IS_ENABLED(CONFIG_VIDEO_TEGRA_TPG))
 639                                 v4l2_async_unregister_subdev(subdev);
 640                         media_entity_cleanup(&subdev->entity);
 641                 }
 642
 643                 list_del(&chan->list);
 644                 kfree(chan);
 645         }
 646 }
 647
 648 static int __maybe_unused csi_runtime_suspend(struct device *dev)
 649 {
 650         struct tegra_csi *csi = dev_get_drvdata(dev);
 651
 652         clk_bulk_disable_unprepare(csi->soc->num_clks, csi->clks);
 653
 654         return 0;
 655 }
 656
 657 static int __maybe_unused csi_runtime_resume(struct device *dev)
 658 {
 659         struct tegra_csi *csi = dev_get_drvdata(dev);
 660         int ret;
 661
 662         ret = clk_bulk_prepare_enable(csi->soc->num_clks, csi->clks);
 663         if (ret < 0) {
 664                 dev_err(csi->dev, "failed to enable clocks: %d\n", ret);
 665                 return ret;
 666         }
 667
 668         return 0;
 669 }
 670
 671 static int tegra_csi_init(struct host1x_client *client)
 672 {
 673         struct tegra_csi *csi = host1x_client_to_csi(client);
 674         struct tegra_video_device *vid = dev_get_drvdata(client->host);
 675         int ret;
 676
 677         INIT_LIST_HEAD(&csi->csi_chans);
 678
 679         if (IS_ENABLED(CONFIG_VIDEO_TEGRA_TPG))
 680                 ret = tegra_csi_tpg_channels_alloc(csi);
 681         else
 682                 ret = tegra_csi_channels_alloc(csi);
 683         if (ret < 0) {
 684                 dev_err(csi->dev,
 685                         "failed to allocate channels: %d\n", ret);
 686                 goto cleanup;
 687         }
 688
 689         ret = tegra_csi_channels_init(csi);
 690         if (ret < 0)
 691                 goto cleanup;
 692
 693         vid->csi = csi;
 694
 695         return 0;
 696
 697 cleanup:
 698         tegra_csi_channels_cleanup(csi);
 699         return ret;
 700 }
 701
 702 static int tegra_csi_exit(struct host1x_client *client)
 703 {
 704         struct tegra_csi *csi = host1x_client_to_csi(client);
 705
 706         tegra_csi_channels_cleanup(csi);
 707
 708         return 0;
 709 }
 710
 711 static const struct host1x_client_ops csi_client_ops = {
 712         .init = tegra_csi_init,
 713         .exit = tegra_csi_exit,
 714 };
 715
 716 static int tegra_csi_probe(struct platform_device *pdev)
 717 {
 718         struct tegra_csi *csi;
 719         unsigned int i;
 720         int ret;
 721
 722         csi = devm_kzalloc(&pdev->dev, sizeof(*csi), GFP_KERNEL);
 723         if (!csi)
 724                 return -ENOMEM;
 725
 726         csi->iomem = devm_platform_ioremap_resource(pdev, 0);
 727         if (IS_ERR(csi->iomem))
 728                 return PTR_ERR(csi->iomem);
 729
 730         csi->soc = of_device_get_match_data(&pdev->dev);
 731
 732         csi->clks = devm_kcalloc(&pdev->dev, csi->soc->num_clks,
 733                                  sizeof(*csi->clks), GFP_KERNEL);
 734         if (!csi->clks)
 735                 return -ENOMEM;
 736
 737         for (i = 0; i < csi->soc->num_clks; i++)
 738                 csi->clks[i].id = csi->soc->clk_names[i];
 739
 740         ret = devm_clk_bulk_get(&pdev->dev, csi->soc->num_clks, csi->clks);
 741         if (ret) {
 742                 dev_err(&pdev->dev, "failed to get the clocks: %d\n", ret);
 743                 return ret;
 744         }
 745
 746         if (!pdev->dev.pm_domain) {
 747                 ret = -ENOENT;
 748                 dev_warn(&pdev->dev, "PM domain is not attached: %d\n", ret);
 749                 return ret;
 750         }
 751
 752         csi->dev = &pdev->dev;
 753         csi->ops = csi->soc->ops;
 754         platform_set_drvdata(pdev, csi);
 755         pm_runtime_enable(&pdev->dev);
 756
 757         /* initialize host1x interface */
 758         INIT_LIST_HEAD(&csi->client.list);
 759         csi->client.ops = &csi_client_ops;
 760         csi->client.dev = &pdev->dev;
 761
 762         ret = host1x_client_register(&csi->client);
 763         if (ret < 0) {
 764                 dev_err(&pdev->dev,
 765                         "failed to register host1x client: %d\n", ret);
 766                 goto rpm_disable;
 767         }
 768
 769         return 0;
 770
 771 rpm_disable:
 772         pm_runtime_disable(&pdev->dev);
 773         return ret;
 774 }
 775
 776 static int tegra_csi_remove(struct platform_device *pdev)
 777 {
 778         struct tegra_csi *csi = platform_get_drvdata(pdev);
 779         int err;
 780
 781         err = host1x_client_unregister(&csi->client);
 782         if (err < 0) {
 783                 dev_err(&pdev->dev,
 784                         "failed to unregister host1x client: %d\n", err);
 785                 return err;
 786         }
 787
 788         pm_runtime_disable(&pdev->dev);
 789
 790         return 0;
 791 }
 792
 793 static const struct of_device_id tegra_csi_of_id_table[] = {
 794 #if defined(CONFIG_ARCH_TEGRA_210_SOC)
 795         { .compatible = "nvidia,tegra210-csi", .data = &tegra210_csi_soc },
 796 #endif
 797         { }
 798 };
 799 MODULE_DEVICE_TABLE(of, tegra_csi_of_id_table);
 800
 801 static const struct dev_pm_ops tegra_csi_pm_ops = {
 802         SET_RUNTIME_PM_OPS(csi_runtime_suspend, csi_runtime_resume, NULL)
 803 };
 804
 805 struct platform_driver tegra_csi_driver = {
 806         .driver = {
 807                 .name           = "tegra-csi",
 808                 .of_match_table = tegra_csi_of_id_table,
 809                 .pm             = &tegra_csi_pm_ops,
 810         },
 811         .probe                  = tegra_csi_probe,
 812         .remove                 = tegra_csi_remove,
 813 };