drivers/media/platform/qcom/camss/camss.c

   1 // SPDX-License-Identifier: GPL-2.0
   2 /*
   3  * camss.c
   4  *
   5  * Qualcomm MSM Camera Subsystem - Core
   6  *
   7  * Copyright (c) 2015, The Linux Foundation. All rights reserved.
   8  * Copyright (C) 2015-2018 Linaro Ltd.
   9  */
  10 #include <linux/clk.h>
  11 #include <linux/media-bus-format.h>
  12 #include <linux/media.h>
  13 #include <linux/module.h>
  14 #include <linux/platform_device.h>
  15 #include <linux/of.h>
  16 #include <linux/of_graph.h>
  17 #include <linux/pm_runtime.h>
  18 #include <linux/pm_domain.h>
  19 #include <linux/slab.h>
  20 #include <linux/videodev2.h>
  21
  22 #include <media/media-device.h>
  23 #include <media/v4l2-async.h>
  24 #include <media/v4l2-device.h>
  25 #include <media/v4l2-mc.h>
  26 #include <media/v4l2-fwnode.h>
  27
  28 #include "camss.h"
  29
  30 #define CAMSS_CLOCK_MARGIN_NUMERATOR 105
  31 #define CAMSS_CLOCK_MARGIN_DENOMINATOR 100
  32
  33 static const struct resources csiphy_res_8x16[] = {
  34         /* CSIPHY0 */
  35         {
  36                 .regulator = { NULL },
  37                 .clock = { "top_ahb", "ispif_ahb", "ahb", "csiphy0_timer" },
  38                 .clock_rate = { { 0 },
  39                                 { 0 },
  40                                 { 0 },
  41                                 { 100000000, 200000000 } },
  42                 .reg = { "csiphy0", "csiphy0_clk_mux" },
  43                 .interrupt = { "csiphy0" }
  44         },
  45
  46         /* CSIPHY1 */
  47         {
  48                 .regulator = { NULL },
  49                 .clock = { "top_ahb", "ispif_ahb", "ahb", "csiphy1_timer" },
  50                 .clock_rate = { { 0 },
  51                                 { 0 },
  52                                 { 0 },
  53                                 { 100000000, 200000000 } },
  54                 .reg = { "csiphy1", "csiphy1_clk_mux" },
  55                 .interrupt = { "csiphy1" }
  56         }
  57 };
  58
  59 static const struct resources csid_res_8x16[] = {
  60         /* CSID0 */
  61         {
  62                 .regulator = { "vdda" },
  63                 .clock = { "top_ahb", "ispif_ahb", "csi0_ahb", "ahb",
  64                            "csi0", "csi0_phy", "csi0_pix", "csi0_rdi" },
  65                 .clock_rate = { { 0 },
  66                                 { 0 },
  67                                 { 0 },
  68                                 { 0 },
  69                                 { 100000000, 200000000 },
  70                                 { 0 },
  71                                 { 0 },
  72                                 { 0 } },
  73                 .reg = { "csid0" },
  74                 .interrupt = { "csid0" }
  75         },
  76
  77         /* CSID1 */
  78         {
  79                 .regulator = { "vdda" },
  80                 .clock = { "top_ahb", "ispif_ahb", "csi1_ahb", "ahb",
  81                            "csi1", "csi1_phy", "csi1_pix", "csi1_rdi" },
  82                 .clock_rate = { { 0 },
  83                                 { 0 },
  84                                 { 0 },
  85                                 { 0 },
  86                                 { 100000000, 200000000 },
  87                                 { 0 },
  88                                 { 0 },
  89                                 { 0 } },
  90                 .reg = { "csid1" },
  91                 .interrupt = { "csid1" }
  92         },
  93 };
  94
  95 static const struct resources_ispif ispif_res_8x16 = {
  96         /* ISPIF */
  97         .clock = { "top_ahb", "ahb", "ispif_ahb",
  98                    "csi0", "csi0_pix", "csi0_rdi",
  99                    "csi1", "csi1_pix", "csi1_rdi" },
 100         .clock_for_reset = { "vfe0", "csi_vfe0" },
 101         .reg = { "ispif", "csi_clk_mux" },
 102         .interrupt = "ispif"
 103
 104 };
 105
 106 static const struct resources vfe_res_8x16[] = {
 107         /* VFE0 */
 108         {
 109                 .regulator = { NULL },
 110                 .clock = { "top_ahb", "vfe0", "csi_vfe0",
 111                            "vfe_ahb", "vfe_axi", "ahb" },
 112                 .clock_rate = { { 0 },
 113                                 { 50000000, 80000000, 100000000, 160000000,
 114                                   177780000, 200000000, 266670000, 320000000,
 115                                   400000000, 465000000 },
 116                                 { 0 },
 117                                 { 0 },
 118                                 { 0 },
 119                                 { 0 },
 120                                 { 0 },
 121                                 { 0 },
 122                                 { 0 } },
 123                 .reg = { "vfe0" },
 124                 .interrupt = { "vfe0" }
 125         }
 126 };
 127
 128 static const struct resources csiphy_res_8x96[] = {
 129         /* CSIPHY0 */
 130         {
 131                 .regulator = { NULL },
 132                 .clock = { "top_ahb", "ispif_ahb", "ahb", "csiphy0_timer" },
 133                 .clock_rate = { { 0 },
 134                                 { 0 },
 135                                 { 0 },
 136                                 { 100000000, 200000000, 266666667 } },
 137                 .reg = { "csiphy0", "csiphy0_clk_mux" },
 138                 .interrupt = { "csiphy0" }
 139         },
 140
 141         /* CSIPHY1 */
 142         {
 143                 .regulator = { NULL },
 144                 .clock = { "top_ahb", "ispif_ahb", "ahb", "csiphy1_timer" },
 145                 .clock_rate = { { 0 },
 146                                 { 0 },
 147                                 { 0 },
 148                                 { 100000000, 200000000, 266666667 } },
 149                 .reg = { "csiphy1", "csiphy1_clk_mux" },
 150                 .interrupt = { "csiphy1" }
 151         },
 152
 153         /* CSIPHY2 */
 154         {
 155                 .regulator = { NULL },
 156                 .clock = { "top_ahb", "ispif_ahb", "ahb", "csiphy2_timer" },
 157                 .clock_rate = { { 0 },
 158                                 { 0 },
 159                                 { 0 },
 160                                 { 100000000, 200000000, 266666667 } },
 161                 .reg = { "csiphy2", "csiphy2_clk_mux" },
 162                 .interrupt = { "csiphy2" }
 163         }
 164 };
 165
 166 static const struct resources csid_res_8x96[] = {
 167         /* CSID0 */
 168         {
 169                 .regulator = { "vdda" },
 170                 .clock = { "top_ahb", "ispif_ahb", "csi0_ahb", "ahb",
 171                            "csi0", "csi0_phy", "csi0_pix", "csi0_rdi" },
 172                 .clock_rate = { { 0 },
 173                                 { 0 },
 174                                 { 0 },
 175                                 { 0 },
 176                                 { 100000000, 200000000, 266666667 },
 177                                 { 0 },
 178                                 { 0 },
 179                                 { 0 } },
 180                 .reg = { "csid0" },
 181                 .interrupt = { "csid0" }
 182         },
 183
 184         /* CSID1 */
 185         {
 186                 .regulator = { "vdda" },
 187                 .clock = { "top_ahb", "ispif_ahb", "csi1_ahb", "ahb",
 188                            "csi1", "csi1_phy", "csi1_pix", "csi1_rdi" },
 189                 .clock_rate = { { 0 },
 190                                 { 0 },
 191                                 { 0 },
 192                                 { 0 },
 193                                 { 100000000, 200000000, 266666667 },
 194                                 { 0 },
 195                                 { 0 },
 196                                 { 0 } },
 197                 .reg = { "csid1" },
 198                 .interrupt = { "csid1" }
 199         },
 200
 201         /* CSID2 */
 202         {
 203                 .regulator = { "vdda" },
 204                 .clock = { "top_ahb", "ispif_ahb", "csi2_ahb", "ahb",
 205                            "csi2", "csi2_phy", "csi2_pix", "csi2_rdi" },
 206                 .clock_rate = { { 0 },
 207                                 { 0 },
 208                                 { 0 },
 209                                 { 0 },
 210                                 { 100000000, 200000000, 266666667 },
 211                                 { 0 },
 212                                 { 0 },
 213                                 { 0 } },
 214                 .reg = { "csid2" },
 215                 .interrupt = { "csid2" }
 216         },
 217
 218         /* CSID3 */
 219         {
 220                 .regulator = { "vdda" },
 221                 .clock = { "top_ahb", "ispif_ahb", "csi3_ahb", "ahb",
 222                            "csi3", "csi3_phy", "csi3_pix", "csi3_rdi" },
 223                 .clock_rate = { { 0 },
 224                                 { 0 },
 225                                 { 0 },
 226                                 { 0 },
 227                                 { 100000000, 200000000, 266666667 },
 228                                 { 0 },
 229                                 { 0 },
 230                                 { 0 } },
 231                 .reg = { "csid3" },
 232                 .interrupt = { "csid3" }
 233         }
 234 };
 235
 236 static const struct resources_ispif ispif_res_8x96 = {
 237         /* ISPIF */
 238         .clock = { "top_ahb", "ahb", "ispif_ahb",
 239                    "csi0", "csi0_pix", "csi0_rdi",
 240                    "csi1", "csi1_pix", "csi1_rdi",
 241                    "csi2", "csi2_pix", "csi2_rdi",
 242                    "csi3", "csi3_pix", "csi3_rdi" },
 243         .clock_for_reset = { "vfe0", "csi_vfe0", "vfe1", "csi_vfe1" },
 244         .reg = { "ispif", "csi_clk_mux" },
 245         .interrupt = "ispif"
 246 };
 247
 248 static const struct resources vfe_res_8x96[] = {
 249         /* VFE0 */
 250         {
 251                 .regulator = { NULL },
 252                 .clock = { "top_ahb", "ahb", "vfe0", "csi_vfe0", "vfe_ahb",
 253                            "vfe0_ahb", "vfe_axi", "vfe0_stream"},
 254                 .clock_rate = { { 0 },
 255                                 { 0 },
 256                                 { 75000000, 100000000, 300000000,
 257                                   320000000, 480000000, 600000000 },
 258                                 { 0 },
 259                                 { 0 },
 260                                 { 0 },
 261                                 { 0 },
 262                                 { 0 } },
 263                 .reg = { "vfe0" },
 264                 .interrupt = { "vfe0" }
 265         },
 266
 267         /* VFE1 */
 268         {
 269                 .regulator = { NULL },
 270                 .clock = { "top_ahb", "ahb", "vfe1", "csi_vfe1", "vfe_ahb",
 271                            "vfe1_ahb", "vfe_axi", "vfe1_stream"},
 272                 .clock_rate = { { 0 },
 273                                 { 0 },
 274                                 { 75000000, 100000000, 300000000,
 275                                   320000000, 480000000, 600000000 },
 276                                 { 0 },
 277                                 { 0 },
 278                                 { 0 },
 279                                 { 0 },
 280                                 { 0 } },
 281                 .reg = { "vfe1" },
 282                 .interrupt = { "vfe1" }
 283         }
 284 };
 285
 286 /*
 287  * camss_add_clock_margin - Add margin to clock frequency rate
 288  * @rate: Clock frequency rate
 289  *
 290  * When making calculations with physical clock frequency values
 291  * some safety margin must be added. Add it.
 292  */
 293 inline void camss_add_clock_margin(u64 *rate)
 294 {
 295         *rate *= CAMSS_CLOCK_MARGIN_NUMERATOR;
 296         *rate = div_u64(*rate, CAMSS_CLOCK_MARGIN_DENOMINATOR);
 297 }
 298
 299 /*
 300  * camss_enable_clocks - Enable multiple clocks
 301  * @nclocks: Number of clocks in clock array
 302  * @clock: Clock array
 303  * @dev: Device
 304  *
 305  * Return 0 on success or a negative error code otherwise
 306  */
 307 int camss_enable_clocks(int nclocks, struct camss_clock *clock,
 308                         struct device *dev)
 309 {
 310         int ret;
 311         int i;
 312
 313         for (i = 0; i < nclocks; i++) {
 314                 ret = clk_prepare_enable(clock[i].clk);
 315                 if (ret) {
 316                         dev_err(dev, "clock enable failed: %d\n", ret);
 317                         goto error;
 318                 }
 319         }
 320
 321         return 0;
 322
 323 error:
 324         for (i--; i >= 0; i--)
 325                 clk_disable_unprepare(clock[i].clk);
 326
 327         return ret;
 328 }
 329
 330 /*
 331  * camss_disable_clocks - Disable multiple clocks
 332  * @nclocks: Number of clocks in clock array
 333  * @clock: Clock array
 334  */
 335 void camss_disable_clocks(int nclocks, struct camss_clock *clock)
 336 {
 337         int i;
 338
 339         for (i = nclocks - 1; i >= 0; i--)
 340                 clk_disable_unprepare(clock[i].clk);
 341 }
 342
 343 /*
 344  * camss_find_sensor - Find a linked media entity which represents a sensor
 345  * @entity: Media entity to start searching from
 346  *
 347  * Return a pointer to sensor media entity or NULL if not found
 348  */
 349 struct media_entity *camss_find_sensor(struct media_entity *entity)
 350 {
 351         struct media_pad *pad;
 352
 353         while (1) {
 354                 pad = &entity->pads[0];
 355                 if (!(pad->flags & MEDIA_PAD_FL_SINK))
 356                         return NULL;
 357
 358                 pad = media_entity_remote_pad(pad);
 359                 if (!pad || !is_media_entity_v4l2_subdev(pad->entity))
 360                         return NULL;
 361
 362                 entity = pad->entity;
 363
 364                 if (entity->function == MEDIA_ENT_F_CAM_SENSOR)
 365                         return entity;
 366         }
 367 }
 368
 369 /*
 370  * camss_get_pixel_clock - Get pixel clock rate from sensor
 371  * @entity: Media entity in the current pipeline
 372  * @pixel_clock: Received pixel clock value
 373  *
 374  * Return 0 on success or a negative error code otherwise
 375  */
 376 int camss_get_pixel_clock(struct media_entity *entity, u32 *pixel_clock)
 377 {
 378         struct media_entity *sensor;
 379         struct v4l2_subdev *subdev;
 380         struct v4l2_ctrl *ctrl;
 381
 382         sensor = camss_find_sensor(entity);
 383         if (!sensor)
 384                 return -ENODEV;
 385
 386         subdev = media_entity_to_v4l2_subdev(sensor);
 387
 388         ctrl = v4l2_ctrl_find(subdev->ctrl_handler, V4L2_CID_PIXEL_RATE);
 389
 390         if (!ctrl)
 391                 return -EINVAL;
 392
 393         *pixel_clock = v4l2_ctrl_g_ctrl_int64(ctrl);
 394
 395         return 0;
 396 }
 397
 398 int camss_pm_domain_on(struct camss *camss, int id)
 399 {
 400         if (camss->version == CAMSS_8x96) {
 401                 camss->genpd_link[id] = device_link_add(camss->dev,
 402                                 camss->genpd[id], DL_FLAG_STATELESS |
 403                                 DL_FLAG_PM_RUNTIME | DL_FLAG_RPM_ACTIVE);
 404
 405                 if (!camss->genpd_link[id])
 406                         return -EINVAL;
 407         }
 408
 409         return 0;
 410 }
 411
 412 void camss_pm_domain_off(struct camss *camss, int id)
 413 {
 414         if (camss->version == CAMSS_8x96)
 415                 device_link_del(camss->genpd_link[id]);
 416 }
 417
 418 /*
 419  * camss_of_parse_endpoint_node - Parse port endpoint node
 420  * @dev: Device
 421  * @node: Device node to be parsed
 422  * @csd: Parsed data from port endpoint node
 423  *
 424  * Return 0 on success or a negative error code on failure
 425  */
 426 static int camss_of_parse_endpoint_node(struct device *dev,
 427                                         struct device_node *node,
 428                                         struct camss_async_subdev *csd)
 429 {
 430         struct csiphy_lanes_cfg *lncfg = &csd->interface.csi2.lane_cfg;
 431         struct v4l2_fwnode_bus_mipi_csi2 *mipi_csi2;
 432         struct v4l2_fwnode_endpoint vep = { { 0 } };
 433         unsigned int i;
 434
 435         v4l2_fwnode_endpoint_parse(of_fwnode_handle(node), &vep);
 436
 437         csd->interface.csiphy_id = vep.base.port;
 438
 439         mipi_csi2 = &vep.bus.mipi_csi2;
 440         lncfg->clk.pos = mipi_csi2->clock_lane;
 441         lncfg->clk.pol = mipi_csi2->lane_polarities[0];
 442         lncfg->num_data = mipi_csi2->num_data_lanes;
 443
 444         lncfg->data = devm_kcalloc(dev,
 445                                    lncfg->num_data, sizeof(*lncfg->data),
 446                                    GFP_KERNEL);
 447         if (!lncfg->data)
 448                 return -ENOMEM;
 449
 450         for (i = 0; i < lncfg->num_data; i++) {
 451                 lncfg->data[i].pos = mipi_csi2->data_lanes[i];
 452                 lncfg->data[i].pol = mipi_csi2->lane_polarities[i + 1];
 453         }
 454
 455         return 0;
 456 }
 457
 458 /*
 459  * camss_of_parse_ports - Parse ports node
 460  * @dev: Device
 461  * @notifier: v4l2_device notifier data
 462  *
 463  * Return number of "port" nodes found in "ports" node
 464  */
 465 static int camss_of_parse_ports(struct camss *camss)
 466 {
 467         struct device *dev = camss->dev;
 468         struct device_node *node = NULL;
 469         struct device_node *remote = NULL;
 470         int ret, num_subdevs = 0;
 471
 472         for_each_endpoint_of_node(dev->of_node, node) {
 473                 struct camss_async_subdev *csd;
 474                 struct v4l2_async_subdev *asd;
 475
 476                 if (!of_device_is_available(node))
 477                         continue;
 478
 479                 remote = of_graph_get_remote_port_parent(node);
 480                 if (!remote) {
 481                         dev_err(dev, "Cannot get remote parent\n");
 482                         ret = -EINVAL;
 483                         goto err_cleanup;
 484                 }
 485
 486                 asd = v4l2_async_notifier_add_fwnode_subdev(
 487                         &camss->notifier, of_fwnode_handle(remote),
 488                         sizeof(*csd));
 489                 if (IS_ERR(asd)) {
 490                         ret = PTR_ERR(asd);
 491                         of_node_put(remote);
 492                         goto err_cleanup;
 493                 }
 494
 495                 csd = container_of(asd, struct camss_async_subdev, asd);
 496
 497                 ret = camss_of_parse_endpoint_node(dev, node, csd);
 498                 if (ret < 0)
 499                         goto err_cleanup;
 500
 501                 num_subdevs++;
 502         }
 503
 504         return num_subdevs;
 505
 506 err_cleanup:
 507         v4l2_async_notifier_cleanup(&camss->notifier);
 508         of_node_put(node);
 509         return ret;
 510 }
 511
 512 /*
 513  * camss_init_subdevices - Initialize subdev structures and resources
 514  * @camss: CAMSS device
 515  *
 516  * Return 0 on success or a negative error code on failure
 517  */
 518 static int camss_init_subdevices(struct camss *camss)
 519 {
 520         const struct resources *csiphy_res;
 521         const struct resources *csid_res;
 522         const struct resources_ispif *ispif_res;
 523         const struct resources *vfe_res;
 524         unsigned int i;
 525         int ret;
 526
 527         if (camss->version == CAMSS_8x16) {
 528                 csiphy_res = csiphy_res_8x16;
 529                 csid_res = csid_res_8x16;
 530                 ispif_res = &ispif_res_8x16;
 531                 vfe_res = vfe_res_8x16;
 532         } else if (camss->version == CAMSS_8x96) {
 533                 csiphy_res = csiphy_res_8x96;
 534                 csid_res = csid_res_8x96;
 535                 ispif_res = &ispif_res_8x96;
 536                 vfe_res = vfe_res_8x96;
 537         } else {
 538                 return -EINVAL;
 539         }
 540
 541         for (i = 0; i < camss->csiphy_num; i++) {
 542                 ret = msm_csiphy_subdev_init(camss, &camss->csiphy[i],
 543                                              &csiphy_res[i], i);
 544                 if (ret < 0) {
 545                         dev_err(camss->dev,
 546                                 "Failed to init csiphy%d sub-device: %d\n",
 547                                 i, ret);
 548                         return ret;
 549                 }
 550         }
 551
 552         for (i = 0; i < camss->csid_num; i++) {
 553                 ret = msm_csid_subdev_init(camss, &camss->csid[i],
 554                                            &csid_res[i], i);
 555                 if (ret < 0) {
 556                         dev_err(camss->dev,
 557                                 "Failed to init csid%d sub-device: %d\n",
 558                                 i, ret);
 559                         return ret;
 560                 }
 561         }
 562
 563         ret = msm_ispif_subdev_init(&camss->ispif, ispif_res);
 564         if (ret < 0) {
 565                 dev_err(camss->dev, "Failed to init ispif sub-device: %d\n",
 566                         ret);
 567                 return ret;
 568         }
 569
 570         for (i = 0; i < camss->vfe_num; i++) {
 571                 ret = msm_vfe_subdev_init(camss, &camss->vfe[i],
 572                                           &vfe_res[i], i);
 573                 if (ret < 0) {
 574                         dev_err(camss->dev,
 575                                 "Fail to init vfe%d sub-device: %d\n", i, ret);
 576                         return ret;
 577                 }
 578         }
 579
 580         return 0;
 581 }
 582
 583 /*
 584  * camss_register_entities - Register subdev nodes and create links
 585  * @camss: CAMSS device
 586  *
 587  * Return 0 on success or a negative error code on failure
 588  */
 589 static int camss_register_entities(struct camss *camss)
 590 {
 591         int i, j, k;
 592         int ret;
 593
 594         for (i = 0; i < camss->csiphy_num; i++) {
 595                 ret = msm_csiphy_register_entity(&camss->csiphy[i],
 596                                                  &camss->v4l2_dev);
 597                 if (ret < 0) {
 598                         dev_err(camss->dev,
 599                                 "Failed to register csiphy%d entity: %d\n",
 600                                 i, ret);
 601                         goto err_reg_csiphy;
 602                 }
 603         }
 604
 605         for (i = 0; i < camss->csid_num; i++) {
 606                 ret = msm_csid_register_entity(&camss->csid[i],
 607                                                &camss->v4l2_dev);
 608                 if (ret < 0) {
 609                         dev_err(camss->dev,
 610                                 "Failed to register csid%d entity: %d\n",
 611                                 i, ret);
 612                         goto err_reg_csid;
 613                 }
 614         }
 615
 616         ret = msm_ispif_register_entities(&camss->ispif, &camss->v4l2_dev);
 617         if (ret < 0) {
 618                 dev_err(camss->dev, "Failed to register ispif entities: %d\n",
 619                         ret);
 620                 goto err_reg_ispif;
 621         }
 622
 623         for (i = 0; i < camss->vfe_num; i++) {
 624                 ret = msm_vfe_register_entities(&camss->vfe[i],
 625                                                 &camss->v4l2_dev);
 626                 if (ret < 0) {
 627                         dev_err(camss->dev,
 628                                 "Failed to register vfe%d entities: %d\n",
 629                                 i, ret);
 630                         goto err_reg_vfe;
 631                 }
 632         }
 633
 634         for (i = 0; i < camss->csiphy_num; i++) {
 635                 for (j = 0; j < camss->csid_num; j++) {
 636                         ret = media_create_pad_link(
 637                                 &camss->csiphy[i].subdev.entity,
 638                                 MSM_CSIPHY_PAD_SRC,
 639                                 &camss->csid[j].subdev.entity,
 640                                 MSM_CSID_PAD_SINK,
 641                                 0);
 642                         if (ret < 0) {
 643                                 dev_err(camss->dev,
 644                                         "Failed to link %s->%s entities: %d\n",
 645                                         camss->csiphy[i].subdev.entity.name,
 646                                         camss->csid[j].subdev.entity.name,
 647                                         ret);
 648                                 goto err_link;
 649                         }
 650                 }
 651         }
 652
 653         for (i = 0; i < camss->csid_num; i++) {
 654                 for (j = 0; j < camss->ispif.line_num; j++) {
 655                         ret = media_create_pad_link(
 656                                 &camss->csid[i].subdev.entity,
 657                                 MSM_CSID_PAD_SRC,
 658                                 &camss->ispif.line[j].subdev.entity,
 659                                 MSM_ISPIF_PAD_SINK,
 660                                 0);
 661                         if (ret < 0) {
 662                                 dev_err(camss->dev,
 663                                         "Failed to link %s->%s entities: %d\n",
 664                                         camss->csid[i].subdev.entity.name,
 665                                         camss->ispif.line[j].subdev.entity.name,
 666                                         ret);
 667                                 goto err_link;
 668                         }
 669                 }
 670         }
 671
 672         for (i = 0; i < camss->ispif.line_num; i++)
 673                 for (k = 0; k < camss->vfe_num; k++)
 674                         for (j = 0; j < ARRAY_SIZE(camss->vfe[k].line); j++) {
 675                                 ret = media_create_pad_link(
 676                                         &camss->ispif.line[i].subdev.entity,
 677                                         MSM_ISPIF_PAD_SRC,
 678                                         &camss->vfe[k].line[j].subdev.entity,
 679                                         MSM_VFE_PAD_SINK,
 680                                         0);
 681                                 if (ret < 0) {
 682                                         dev_err(camss->dev,
 683                                                 "Failed to link %s->%s entities: %d\n",
 684                                                 camss->ispif.line[i].subdev.entity.name,
 685                                                 camss->vfe[k].line[j].subdev.entity.name,
 686                                                 ret);
 687                                         goto err_link;
 688                                 }
 689                         }
 690
 691         return 0;
 692
 693 err_link:
 694         i = camss->vfe_num;
 695 err_reg_vfe:
 696         for (i--; i >= 0; i--)
 697                 msm_vfe_unregister_entities(&camss->vfe[i]);
 698
 699         msm_ispif_unregister_entities(&camss->ispif);
 700 err_reg_ispif:
 701
 702         i = camss->csid_num;
 703 err_reg_csid:
 704         for (i--; i >= 0; i--)
 705                 msm_csid_unregister_entity(&camss->csid[i]);
 706
 707         i = camss->csiphy_num;
 708 err_reg_csiphy:
 709         for (i--; i >= 0; i--)
 710                 msm_csiphy_unregister_entity(&camss->csiphy[i]);
 711
 712         return ret;
 713 }
 714
 715 /*
 716  * camss_unregister_entities - Unregister subdev nodes
 717  * @camss: CAMSS device
 718  *
 719  * Return 0 on success or a negative error code on failure
 720  */
 721 static void camss_unregister_entities(struct camss *camss)
 722 {
 723         unsigned int i;
 724
 725         for (i = 0; i < camss->csiphy_num; i++)
 726                 msm_csiphy_unregister_entity(&camss->csiphy[i]);
 727
 728         for (i = 0; i < camss->csid_num; i++)
 729                 msm_csid_unregister_entity(&camss->csid[i]);
 730
 731         msm_ispif_unregister_entities(&camss->ispif);
 732
 733         for (i = 0; i < camss->vfe_num; i++)
 734                 msm_vfe_unregister_entities(&camss->vfe[i]);
 735 }
 736
 737 static int camss_subdev_notifier_bound(struct v4l2_async_notifier *async,
 738                                        struct v4l2_subdev *subdev,
 739                                        struct v4l2_async_subdev *asd)
 740 {
 741         struct camss *camss = container_of(async, struct camss, notifier);
 742         struct camss_async_subdev *csd =
 743                 container_of(asd, struct camss_async_subdev, asd);
 744         u8 id = csd->interface.csiphy_id;
 745         struct csiphy_device *csiphy = &camss->csiphy[id];
 746
 747         csiphy->cfg.csi2 = &csd->interface.csi2;
 748         subdev->host_priv = csiphy;
 749
 750         return 0;
 751 }
 752
 753 static int camss_subdev_notifier_complete(struct v4l2_async_notifier *async)
 754 {
 755         struct camss *camss = container_of(async, struct camss, notifier);
 756         struct v4l2_device *v4l2_dev = &camss->v4l2_dev;
 757         struct v4l2_subdev *sd;
 758         int ret;
 759
 760         list_for_each_entry(sd, &v4l2_dev->subdevs, list) {
 761                 if (sd->host_priv) {
 762                         struct media_entity *sensor = &sd->entity;
 763                         struct csiphy_device *csiphy =
 764                                         (struct csiphy_device *) sd->host_priv;
 765                         struct media_entity *input = &csiphy->subdev.entity;
 766                         unsigned int i;
 767
 768                         for (i = 0; i < sensor->num_pads; i++) {
 769                                 if (sensor->pads[i].flags & MEDIA_PAD_FL_SOURCE)
 770                                         break;
 771                         }
 772                         if (i == sensor->num_pads) {
 773                                 dev_err(camss->dev,
 774                                         "No source pad in external entity\n");
 775                                 return -EINVAL;
 776                         }
 777
 778                         ret = media_create_pad_link(sensor, i,
 779                                 input, MSM_CSIPHY_PAD_SINK,
 780                                 MEDIA_LNK_FL_IMMUTABLE | MEDIA_LNK_FL_ENABLED);
 781                         if (ret < 0) {
 782                                 dev_err(camss->dev,
 783                                         "Failed to link %s->%s entities: %d\n",
 784                                         sensor->name, input->name, ret);
 785                                 return ret;
 786                         }
 787                 }
 788         }
 789
 790         ret = v4l2_device_register_subdev_nodes(&camss->v4l2_dev);
 791         if (ret < 0)
 792                 return ret;
 793
 794         return media_device_register(&camss->media_dev);
 795 }
 796
 797 static const struct v4l2_async_notifier_operations camss_subdev_notifier_ops = {
 798         .bound = camss_subdev_notifier_bound,
 799         .complete = camss_subdev_notifier_complete,
 800 };
 801
 802 static const struct media_device_ops camss_media_ops = {
 803         .link_notify = v4l2_pipeline_link_notify,
 804 };
 805
 806 /*
 807  * camss_probe - Probe CAMSS platform device
 808  * @pdev: Pointer to CAMSS platform device
 809  *
 810  * Return 0 on success or a negative error code on failure
 811  */
 812 static int camss_probe(struct platform_device *pdev)
 813 {
 814         struct device *dev = &pdev->dev;
 815         struct camss *camss;
 816         int num_subdevs, ret;
 817
 818         camss = kzalloc(sizeof(*camss), GFP_KERNEL);
 819         if (!camss)
 820                 return -ENOMEM;
 821
 822         atomic_set(&camss->ref_count, 0);
 823         camss->dev = dev;
 824         platform_set_drvdata(pdev, camss);
 825
 826         if (of_device_is_compatible(dev->of_node, "qcom,msm8916-camss")) {
 827                 camss->version = CAMSS_8x16;
 828                 camss->csiphy_num = 2;
 829                 camss->csid_num = 2;
 830                 camss->vfe_num = 1;
 831         } else if (of_device_is_compatible(dev->of_node,
 832                                            "qcom,msm8996-camss")) {
 833                 camss->version = CAMSS_8x96;
 834                 camss->csiphy_num = 3;
 835                 camss->csid_num = 4;
 836                 camss->vfe_num = 2;
 837         } else {
 838                 return -EINVAL;
 839         }
 840
 841         camss->csiphy = devm_kcalloc(dev, camss->csiphy_num,
 842                                      sizeof(*camss->csiphy), GFP_KERNEL);
 843         if (!camss->csiphy)
 844                 return -ENOMEM;
 845
 846         camss->csid = devm_kcalloc(dev, camss->csid_num, sizeof(*camss->csid),
 847                                    GFP_KERNEL);
 848         if (!camss->csid)
 849                 return -ENOMEM;
 850
 851         camss->vfe = devm_kcalloc(dev, camss->vfe_num, sizeof(*camss->vfe),
 852                                   GFP_KERNEL);
 853         if (!camss->vfe)
 854                 return -ENOMEM;
 855
 856         v4l2_async_notifier_init(&camss->notifier);
 857
 858         num_subdevs = camss_of_parse_ports(camss);
 859         if (num_subdevs < 0)
 860                 return num_subdevs;
 861
 862         ret = camss_init_subdevices(camss);
 863         if (ret < 0)
 864                 goto err_cleanup;
 865
 866         ret = dma_set_mask_and_coherent(dev, 0xffffffff);
 867         if (ret)
 868                 goto err_cleanup;
 869
 870         camss->media_dev.dev = camss->dev;
 871         strscpy(camss->media_dev.model, "Qualcomm Camera Subsystem",
 872                 sizeof(camss->media_dev.model));
 873         camss->media_dev.ops = &camss_media_ops;
 874         media_device_init(&camss->media_dev);
 875
 876         camss->v4l2_dev.mdev = &camss->media_dev;
 877         ret = v4l2_device_register(camss->dev, &camss->v4l2_dev);
 878         if (ret < 0) {
 879                 dev_err(dev, "Failed to register V4L2 device: %d\n", ret);
 880                 goto err_cleanup;
 881         }
 882
 883         ret = camss_register_entities(camss);
 884         if (ret < 0)
 885                 goto err_register_entities;
 886
 887         if (num_subdevs) {
 888                 camss->notifier.ops = &camss_subdev_notifier_ops;
 889
 890                 ret = v4l2_async_notifier_register(&camss->v4l2_dev,
 891                                                    &camss->notifier);
 892                 if (ret) {
 893                         dev_err(dev,
 894                                 "Failed to register async subdev nodes: %d\n",
 895                                 ret);
 896                         goto err_register_subdevs;
 897                 }
 898         } else {
 899                 ret = v4l2_device_register_subdev_nodes(&camss->v4l2_dev);
 900                 if (ret < 0) {
 901                         dev_err(dev, "Failed to register subdev nodes: %d\n",
 902                                 ret);
 903                         goto err_register_subdevs;
 904                 }
 905
 906                 ret = media_device_register(&camss->media_dev);
 907                 if (ret < 0) {
 908                         dev_err(dev, "Failed to register media device: %d\n",
 909                                 ret);
 910                         goto err_register_subdevs;
 911                 }
 912         }
 913
 914         if (camss->version == CAMSS_8x96) {
 915                 camss->genpd[PM_DOMAIN_VFE0] = dev_pm_domain_attach_by_id(
 916                                                 camss->dev, PM_DOMAIN_VFE0);
 917                 if (IS_ERR(camss->genpd[PM_DOMAIN_VFE0]))
 918                         return PTR_ERR(camss->genpd[PM_DOMAIN_VFE0]);
 919
 920                 camss->genpd[PM_DOMAIN_VFE1] = dev_pm_domain_attach_by_id(
 921                                                 camss->dev, PM_DOMAIN_VFE1);
 922                 if (IS_ERR(camss->genpd[PM_DOMAIN_VFE1])) {
 923                         dev_pm_domain_detach(camss->genpd[PM_DOMAIN_VFE0],
 924                                              true);
 925                         return PTR_ERR(camss->genpd[PM_DOMAIN_VFE1]);
 926                 }
 927         }
 928
 929         pm_runtime_enable(dev);
 930
 931         return 0;
 932
 933 err_register_subdevs:
 934         camss_unregister_entities(camss);
 935 err_register_entities:
 936         v4l2_device_unregister(&camss->v4l2_dev);
 937 err_cleanup:
 938         v4l2_async_notifier_cleanup(&camss->notifier);
 939
 940         return ret;
 941 }
 942
 943 void camss_delete(struct camss *camss)
 944 {
 945         v4l2_device_unregister(&camss->v4l2_dev);
 946         media_device_unregister(&camss->media_dev);
 947         media_device_cleanup(&camss->media_dev);
 948
 949         pm_runtime_disable(camss->dev);
 950
 951         if (camss->version == CAMSS_8x96) {
 952                 dev_pm_domain_detach(camss->genpd[PM_DOMAIN_VFE0], true);
 953                 dev_pm_domain_detach(camss->genpd[PM_DOMAIN_VFE1], true);
 954         }
 955
 956         kfree(camss);
 957 }
 958
 959 /*
 960  * camss_remove - Remove CAMSS platform device
 961  * @pdev: Pointer to CAMSS platform device
 962  *
 963  * Always returns 0.
 964  */
 965 static int camss_remove(struct platform_device *pdev)
 966 {
 967         unsigned int i;
 968
 969         struct camss *camss = platform_get_drvdata(pdev);
 970
 971         for (i = 0; i < camss->vfe_num; i++)
 972                 msm_vfe_stop_streaming(&camss->vfe[i]);
 973
 974         v4l2_async_notifier_unregister(&camss->notifier);
 975         v4l2_async_notifier_cleanup(&camss->notifier);
 976         camss_unregister_entities(camss);
 977
 978         if (atomic_read(&camss->ref_count) == 0)
 979                 camss_delete(camss);
 980
 981         return 0;
 982 }
 983
 984 static const struct of_device_id camss_dt_match[] = {
 985         { .compatible = "qcom,msm8916-camss" },
 986         { .compatible = "qcom,msm8996-camss" },
 987         { }
 988 };
 989
 990 MODULE_DEVICE_TABLE(of, camss_dt_match);
 991
 992 static int __maybe_unused camss_runtime_suspend(struct device *dev)
 993 {
 994         return 0;
 995 }
 996
 997 static int __maybe_unused camss_runtime_resume(struct device *dev)
 998 {
 999         return 0;
1000 }
1001
1002 static const struct dev_pm_ops camss_pm_ops = {
1003         SET_SYSTEM_SLEEP_PM_OPS(pm_runtime_force_suspend,
1004                                 pm_runtime_force_resume)
1005         SET_RUNTIME_PM_OPS(camss_runtime_suspend, camss_runtime_resume, NULL)
1006 };
1007
1008 static struct platform_driver qcom_camss_driver = {
1009         .probe = camss_probe,
1010         .remove = camss_remove,
1011         .driver = {
1012                 .name = "qcom-camss",
1013                 .of_match_table = camss_dt_match,
1014                 .pm = &camss_pm_ops,
1015         },
1016 };
1017
1018 module_platform_driver(qcom_camss_driver);
1019
1020 MODULE_ALIAS("platform:qcom-camss");
1021 MODULE_DESCRIPTION("Qualcomm Camera Subsystem driver");
1022 MODULE_AUTHOR("Todor Tomov <todor.tomov@linaro.org>");
1023 MODULE_LICENSE("GPL v2");