drivers/media/platform/qcom/camss-8x16/camss.c

   1 /*
   2  * camss.c
   3  *
   4  * Qualcomm MSM Camera Subsystem - Core
   5  *
   6  * Copyright (c) 2015, The Linux Foundation. All rights reserved.
   7  * Copyright (C) 2015-2017 Linaro Ltd.
   8  *
   9  * This program is free software; you can redistribute it and/or modify
  10  * it under the terms of the GNU General Public License version 2 and
  11  * only version 2 as published by the Free Software Foundation.
  12  *
  13  * This program is distributed in the hope that it will be useful,
  14  * but WITHOUT ANY WARRANTY; without even the implied warranty of
  15  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
  16  * GNU General Public License for more details.
  17  */
  18 #include <linux/clk.h>
  19 #include <linux/media-bus-format.h>
  20 #include <linux/media.h>
  21 #include <linux/module.h>
  22 #include <linux/platform_device.h>
  23 #include <linux/of.h>
  24 #include <linux/of_graph.h>
  25 #include <linux/slab.h>
  26 #include <linux/videodev2.h>
  27
  28 #include <media/media-device.h>
  29 #include <media/v4l2-async.h>
  30 #include <media/v4l2-device.h>
  31 #include <media/v4l2-mc.h>
  32 #include <media/v4l2-fwnode.h>
  33
  34 #include "camss.h"
  35
  36 #define CAMSS_CLOCK_MARGIN_NUMERATOR 105
  37 #define CAMSS_CLOCK_MARGIN_DENOMINATOR 100
  38
  39 static const struct resources csiphy_res[] = {
  40         /* CSIPHY0 */
  41         {
  42                 .regulator = { NULL },
  43                 .clock = { "camss_top_ahb", "ispif_ahb",
  44                            "camss_ahb", "csiphy0_timer" },
  45                 .clock_rate = { { 0 },
  46                                 { 0 },
  47                                 { 0 },
  48                                 { 100000000, 200000000 } },
  49                 .reg = { "csiphy0", "csiphy0_clk_mux" },
  50                 .interrupt = { "csiphy0" }
  51         },
  52
  53         /* CSIPHY1 */
  54         {
  55                 .regulator = { NULL },
  56                 .clock = { "camss_top_ahb", "ispif_ahb",
  57                            "camss_ahb", "csiphy1_timer" },
  58                 .clock_rate = { { 0 },
  59                                 { 0 },
  60                                 { 0 },
  61                                 { 100000000, 200000000 } },
  62                 .reg = { "csiphy1", "csiphy1_clk_mux" },
  63                 .interrupt = { "csiphy1" }
  64         }
  65 };
  66
  67 static const struct resources csid_res[] = {
  68         /* CSID0 */
  69         {
  70                 .regulator = { "vdda" },
  71                 .clock = { "camss_top_ahb", "ispif_ahb",
  72                            "csi0_ahb", "camss_ahb",
  73                            "csi0", "csi0_phy", "csi0_pix", "csi0_rdi" },
  74                 .clock_rate = { { 0 },
  75                                 { 0 },
  76                                 { 0 },
  77                                 { 0 },
  78                                 { 100000000, 200000000 },
  79                                 { 0 },
  80                                 { 0 },
  81                                 { 0 } },
  82                 .reg = { "csid0" },
  83                 .interrupt = { "csid0" }
  84         },
  85
  86         /* CSID1 */
  87         {
  88                 .regulator = { "vdda" },
  89                 .clock = { "camss_top_ahb", "ispif_ahb",
  90                            "csi1_ahb", "camss_ahb",
  91                            "csi1", "csi1_phy", "csi1_pix", "csi1_rdi" },
  92                 .clock_rate = { { 0 },
  93                                 { 0 },
  94                                 { 0 },
  95                                 { 0 },
  96                                 { 100000000, 200000000 },
  97                                 { 0 },
  98                                 { 0 },
  99                                 { 0 } },
 100                 .reg = { "csid1" },
 101                 .interrupt = { "csid1" }
 102         },
 103 };
 104
 105 static const struct resources_ispif ispif_res = {
 106         /* ISPIF */
 107         .clock = { "camss_top_ahb", "camss_ahb", "ispif_ahb",
 108                    "csi0", "csi0_pix", "csi0_rdi",
 109                    "csi1", "csi1_pix", "csi1_rdi" },
 110         .clock_for_reset = { "camss_vfe_vfe", "camss_csi_vfe" },
 111         .reg = { "ispif", "csi_clk_mux" },
 112         .interrupt = "ispif"
 113
 114 };
 115
 116 static const struct resources vfe_res = {
 117         /* VFE0 */
 118         .regulator = { NULL },
 119         .clock = { "camss_top_ahb", "camss_vfe_vfe", "camss_csi_vfe",
 120                    "iface", "bus", "camss_ahb" },
 121         .clock_rate = { { 0 },
 122                         { 50000000, 80000000, 100000000, 160000000,
 123                           177780000, 200000000, 266670000, 320000000,
 124                           400000000, 465000000 },
 125                         { 0 },
 126                         { 0 },
 127                         { 0 },
 128                         { 0 },
 129                         { 0 },
 130                         { 0 },
 131                         { 0 } },
 132         .reg = { "vfe0" },
 133         .interrupt = { "vfe0" }
 134 };
 135
 136 /*
 137  * camss_add_clock_margin - Add margin to clock frequency rate
 138  * @rate: Clock frequency rate
 139  *
 140  * When making calculations with physical clock frequency values
 141  * some safety margin must be added. Add it.
 142  */
 143 inline void camss_add_clock_margin(u64 *rate)
 144 {
 145         *rate *= CAMSS_CLOCK_MARGIN_NUMERATOR;
 146         *rate = div_u64(*rate, CAMSS_CLOCK_MARGIN_DENOMINATOR);
 147 }
 148
 149 /*
 150  * camss_enable_clocks - Enable multiple clocks
 151  * @nclocks: Number of clocks in clock array
 152  * @clock: Clock array
 153  * @dev: Device
 154  *
 155  * Return 0 on success or a negative error code otherwise
 156  */
 157 int camss_enable_clocks(int nclocks, struct camss_clock *clock,
 158                         struct device *dev)
 159 {
 160         int ret;
 161         int i;
 162
 163         for (i = 0; i < nclocks; i++) {
 164                 ret = clk_prepare_enable(clock[i].clk);
 165                 if (ret) {
 166                         dev_err(dev, "clock enable failed: %d\n", ret);
 167                         goto error;
 168                 }
 169         }
 170
 171         return 0;
 172
 173 error:
 174         for (i--; i >= 0; i--)
 175                 clk_disable_unprepare(clock[i].clk);
 176
 177         return ret;
 178 }
 179
 180 /*
 181  * camss_disable_clocks - Disable multiple clocks
 182  * @nclocks: Number of clocks in clock array
 183  * @clock: Clock array
 184  */
 185 void camss_disable_clocks(int nclocks, struct camss_clock *clock)
 186 {
 187         int i;
 188
 189         for (i = nclocks - 1; i >= 0; i--)
 190                 clk_disable_unprepare(clock[i].clk);
 191 }
 192
 193 /*
 194  * camss_find_sensor - Find a linked media entity which represents a sensor
 195  * @entity: Media entity to start searching from
 196  *
 197  * Return a pointer to sensor media entity or NULL if not found
 198  */
 199 static struct media_entity *camss_find_sensor(struct media_entity *entity)
 200 {
 201         struct media_pad *pad;
 202
 203         while (1) {
 204                 pad = &entity->pads[0];
 205                 if (!(pad->flags & MEDIA_PAD_FL_SINK))
 206                         return NULL;
 207
 208                 pad = media_entity_remote_pad(pad);
 209                 if (!pad || !is_media_entity_v4l2_subdev(pad->entity))
 210                         return NULL;
 211
 212                 entity = pad->entity;
 213
 214                 if (entity->function == MEDIA_ENT_F_CAM_SENSOR)
 215                         return entity;
 216         }
 217 }
 218
 219 /*
 220  * camss_get_pixel_clock - Get pixel clock rate from sensor
 221  * @entity: Media entity in the current pipeline
 222  * @pixel_clock: Received pixel clock value
 223  *
 224  * Return 0 on success or a negative error code otherwise
 225  */
 226 int camss_get_pixel_clock(struct media_entity *entity, u32 *pixel_clock)
 227 {
 228         struct media_entity *sensor;
 229         struct v4l2_subdev *subdev;
 230         struct v4l2_ctrl *ctrl;
 231
 232         sensor = camss_find_sensor(entity);
 233         if (!sensor)
 234                 return -ENODEV;
 235
 236         subdev = media_entity_to_v4l2_subdev(sensor);
 237
 238         ctrl = v4l2_ctrl_find(subdev->ctrl_handler, V4L2_CID_PIXEL_RATE);
 239
 240         if (!ctrl)
 241                 return -EINVAL;
 242
 243         *pixel_clock = v4l2_ctrl_g_ctrl_int64(ctrl);
 244
 245         return 0;
 246 }
 247
 248 /*
 249  * camss_of_parse_endpoint_node - Parse port endpoint node
 250  * @dev: Device
 251  * @node: Device node to be parsed
 252  * @csd: Parsed data from port endpoint node
 253  *
 254  * Return 0 on success or a negative error code on failure
 255  */
 256 static int camss_of_parse_endpoint_node(struct device *dev,
 257                                         struct device_node *node,
 258                                         struct camss_async_subdev *csd)
 259 {
 260         struct csiphy_lanes_cfg *lncfg = &csd->interface.csi2.lane_cfg;
 261         struct v4l2_fwnode_bus_mipi_csi2 *mipi_csi2;
 262         struct v4l2_fwnode_endpoint vep = { { 0 } };
 263         unsigned int i;
 264
 265         v4l2_fwnode_endpoint_parse(of_fwnode_handle(node), &vep);
 266
 267         csd->interface.csiphy_id = vep.base.port;
 268
 269         mipi_csi2 = &vep.bus.mipi_csi2;
 270         lncfg->clk.pos = mipi_csi2->clock_lane;
 271         lncfg->clk.pol = mipi_csi2->lane_polarities[0];
 272         lncfg->num_data = mipi_csi2->num_data_lanes;
 273
 274         lncfg->data = devm_kcalloc(dev,
 275                                    lncfg->num_data, sizeof(*lncfg->data),
 276                                    GFP_KERNEL);
 277         if (!lncfg->data)
 278                 return -ENOMEM;
 279
 280         for (i = 0; i < lncfg->num_data; i++) {
 281                 lncfg->data[i].pos = mipi_csi2->data_lanes[i];
 282                 lncfg->data[i].pol = mipi_csi2->lane_polarities[i + 1];
 283         }
 284
 285         return 0;
 286 }
 287
 288 /*
 289  * camss_of_parse_ports - Parse ports node
 290  * @dev: Device
 291  * @notifier: v4l2_device notifier data
 292  *
 293  * Return number of "port" nodes found in "ports" node
 294  */
 295 static int camss_of_parse_ports(struct device *dev,
 296                                 struct v4l2_async_notifier *notifier)
 297 {
 298         struct device_node *node = NULL;
 299         struct device_node *remote = NULL;
 300         unsigned int size, i;
 301         int ret;
 302
 303         while ((node = of_graph_get_next_endpoint(dev->of_node, node)))
 304                 if (of_device_is_available(node))
 305                         notifier->num_subdevs++;
 306
 307         size = sizeof(*notifier->subdevs) * notifier->num_subdevs;
 308         notifier->subdevs = devm_kzalloc(dev, size, GFP_KERNEL);
 309         if (!notifier->subdevs) {
 310                 dev_err(dev, "Failed to allocate memory\n");
 311                 return -ENOMEM;
 312         }
 313
 314         i = 0;
 315         while ((node = of_graph_get_next_endpoint(dev->of_node, node))) {
 316                 struct camss_async_subdev *csd;
 317
 318                 if (!of_device_is_available(node))
 319                         continue;
 320
 321                 csd = devm_kzalloc(dev, sizeof(*csd), GFP_KERNEL);
 322                 if (!csd) {
 323                         of_node_put(node);
 324                         dev_err(dev, "Failed to allocate memory\n");
 325                         return -ENOMEM;
 326                 }
 327
 328                 notifier->subdevs[i++] = &csd->asd;
 329
 330                 ret = camss_of_parse_endpoint_node(dev, node, csd);
 331                 if (ret < 0) {
 332                         of_node_put(node);
 333                         return ret;
 334                 }
 335
 336                 remote = of_graph_get_remote_port_parent(node);
 337                 of_node_put(node);
 338
 339                 if (!remote) {
 340                         dev_err(dev, "Cannot get remote parent\n");
 341                         return -EINVAL;
 342                 }
 343
 344                 csd->asd.match_type = V4L2_ASYNC_MATCH_FWNODE;
 345                 csd->asd.match.fwnode = of_fwnode_handle(remote);
 346         }
 347
 348         return notifier->num_subdevs;
 349 }
 350
 351 /*
 352  * camss_init_subdevices - Initialize subdev structures and resources
 353  * @camss: CAMSS device
 354  *
 355  * Return 0 on success or a negative error code on failure
 356  */
 357 static int camss_init_subdevices(struct camss *camss)
 358 {
 359         unsigned int i;
 360         int ret;
 361
 362         for (i = 0; i < ARRAY_SIZE(camss->csiphy); i++) {
 363                 ret = msm_csiphy_subdev_init(&camss->csiphy[i],
 364                                              &csiphy_res[i], i);
 365                 if (ret < 0) {
 366                         dev_err(camss->dev,
 367                                 "Failed to init csiphy%d sub-device: %d\n",
 368                                 i, ret);
 369                         return ret;
 370                 }
 371         }
 372
 373         for (i = 0; i < ARRAY_SIZE(camss->csid); i++) {
 374                 ret = msm_csid_subdev_init(&camss->csid[i],
 375                                            &csid_res[i], i);
 376                 if (ret < 0) {
 377                         dev_err(camss->dev,
 378                                 "Failed to init csid%d sub-device: %d\n",
 379                                 i, ret);
 380                         return ret;
 381                 }
 382         }
 383
 384         ret = msm_ispif_subdev_init(&camss->ispif, &ispif_res);
 385         if (ret < 0) {
 386                 dev_err(camss->dev, "Failed to init ispif sub-device: %d\n",
 387                         ret);
 388                 return ret;
 389         }
 390
 391         ret = msm_vfe_subdev_init(&camss->vfe, &vfe_res);
 392         if (ret < 0) {
 393                 dev_err(camss->dev, "Fail to init vfe sub-device: %d\n", ret);
 394                 return ret;
 395         }
 396
 397         return 0;
 398 }
 399
 400 /*
 401  * camss_register_entities - Register subdev nodes and create links
 402  * @camss: CAMSS device
 403  *
 404  * Return 0 on success or a negative error code on failure
 405  */
 406 static int camss_register_entities(struct camss *camss)
 407 {
 408         int i, j;
 409         int ret;
 410
 411         for (i = 0; i < ARRAY_SIZE(camss->csiphy); i++) {
 412                 ret = msm_csiphy_register_entity(&camss->csiphy[i],
 413                                                  &camss->v4l2_dev);
 414                 if (ret < 0) {
 415                         dev_err(camss->dev,
 416                                 "Failed to register csiphy%d entity: %d\n",
 417                                 i, ret);
 418                         goto err_reg_csiphy;
 419                 }
 420         }
 421
 422         for (i = 0; i < ARRAY_SIZE(camss->csid); i++) {
 423                 ret = msm_csid_register_entity(&camss->csid[i],
 424                                                &camss->v4l2_dev);
 425                 if (ret < 0) {
 426                         dev_err(camss->dev,
 427                                 "Failed to register csid%d entity: %d\n",
 428                                 i, ret);
 429                         goto err_reg_csid;
 430                 }
 431         }
 432
 433         ret = msm_ispif_register_entities(&camss->ispif, &camss->v4l2_dev);
 434         if (ret < 0) {
 435                 dev_err(camss->dev, "Failed to register ispif entities: %d\n",
 436                         ret);
 437                 goto err_reg_ispif;
 438         }
 439
 440         ret = msm_vfe_register_entities(&camss->vfe, &camss->v4l2_dev);
 441         if (ret < 0) {
 442                 dev_err(camss->dev, "Failed to register vfe entities: %d\n",
 443                         ret);
 444                 goto err_reg_vfe;
 445         }
 446
 447         for (i = 0; i < ARRAY_SIZE(camss->csiphy); i++) {
 448                 for (j = 0; j < ARRAY_SIZE(camss->csid); j++) {
 449                         ret = media_create_pad_link(
 450                                 &camss->csiphy[i].subdev.entity,
 451                                 MSM_CSIPHY_PAD_SRC,
 452                                 &camss->csid[j].subdev.entity,
 453                                 MSM_CSID_PAD_SINK,
 454                                 0);
 455                         if (ret < 0) {
 456                                 dev_err(camss->dev,
 457                                         "Failed to link %s->%s entities: %d\n",
 458                                         camss->csiphy[i].subdev.entity.name,
 459                                         camss->csid[j].subdev.entity.name,
 460                                         ret);
 461                                 goto err_link;
 462                         }
 463                 }
 464         }
 465
 466         for (i = 0; i < ARRAY_SIZE(camss->csid); i++) {
 467                 for (j = 0; j < ARRAY_SIZE(camss->ispif.line); j++) {
 468                         ret = media_create_pad_link(
 469                                 &camss->csid[i].subdev.entity,
 470                                 MSM_CSID_PAD_SRC,
 471                                 &camss->ispif.line[j].subdev.entity,
 472                                 MSM_ISPIF_PAD_SINK,
 473                                 0);
 474                         if (ret < 0) {
 475                                 dev_err(camss->dev,
 476                                         "Failed to link %s->%s entities: %d\n",
 477                                         camss->csid[i].subdev.entity.name,
 478                                         camss->ispif.line[j].subdev.entity.name,
 479                                         ret);
 480                                 goto err_link;
 481                         }
 482                 }
 483         }
 484
 485         for (i = 0; i < ARRAY_SIZE(camss->ispif.line); i++) {
 486                 for (j = 0; j < ARRAY_SIZE(camss->vfe.line); j++) {
 487                         ret = media_create_pad_link(
 488                                 &camss->ispif.line[i].subdev.entity,
 489                                 MSM_ISPIF_PAD_SRC,
 490                                 &camss->vfe.line[j].subdev.entity,
 491                                 MSM_VFE_PAD_SINK,
 492                                 0);
 493                         if (ret < 0) {
 494                                 dev_err(camss->dev,
 495                                         "Failed to link %s->%s entities: %d\n",
 496                                         camss->ispif.line[i].subdev.entity.name,
 497                                         camss->vfe.line[j].subdev.entity.name,
 498                                         ret);
 499                                 goto err_link;
 500                         }
 501                 }
 502         }
 503
 504         return 0;
 505
 506 err_link:
 507         msm_vfe_unregister_entities(&camss->vfe);
 508 err_reg_vfe:
 509         msm_ispif_unregister_entities(&camss->ispif);
 510 err_reg_ispif:
 511
 512         i = ARRAY_SIZE(camss->csid);
 513 err_reg_csid:
 514         for (i--; i >= 0; i--)
 515                 msm_csid_unregister_entity(&camss->csid[i]);
 516
 517         i = ARRAY_SIZE(camss->csiphy);
 518 err_reg_csiphy:
 519         for (i--; i >= 0; i--)
 520                 msm_csiphy_unregister_entity(&camss->csiphy[i]);
 521
 522         return ret;
 523 }
 524
 525 /*
 526  * camss_unregister_entities - Unregister subdev nodes
 527  * @camss: CAMSS device
 528  *
 529  * Return 0 on success or a negative error code on failure
 530  */
 531 static void camss_unregister_entities(struct camss *camss)
 532 {
 533         unsigned int i;
 534
 535         for (i = 0; i < ARRAY_SIZE(camss->csiphy); i++)
 536                 msm_csiphy_unregister_entity(&camss->csiphy[i]);
 537
 538         for (i = 0; i < ARRAY_SIZE(camss->csid); i++)
 539                 msm_csid_unregister_entity(&camss->csid[i]);
 540
 541         msm_ispif_unregister_entities(&camss->ispif);
 542         msm_vfe_unregister_entities(&camss->vfe);
 543 }
 544
 545 static int camss_subdev_notifier_bound(struct v4l2_async_notifier *async,
 546                                        struct v4l2_subdev *subdev,
 547                                        struct v4l2_async_subdev *asd)
 548 {
 549         struct camss *camss = container_of(async, struct camss, notifier);
 550         struct camss_async_subdev *csd =
 551                 container_of(asd, struct camss_async_subdev, asd);
 552         u8 id = csd->interface.csiphy_id;
 553         struct csiphy_device *csiphy = &camss->csiphy[id];
 554
 555         csiphy->cfg.csi2 = &csd->interface.csi2;
 556         subdev->host_priv = csiphy;
 557
 558         return 0;
 559 }
 560
 561 static int camss_subdev_notifier_complete(struct v4l2_async_notifier *async)
 562 {
 563         struct camss *camss = container_of(async, struct camss, notifier);
 564         struct v4l2_device *v4l2_dev = &camss->v4l2_dev;
 565         struct v4l2_subdev *sd;
 566         int ret;
 567
 568         list_for_each_entry(sd, &v4l2_dev->subdevs, list) {
 569                 if (sd->host_priv) {
 570                         struct media_entity *sensor = &sd->entity;
 571                         struct csiphy_device *csiphy =
 572                                         (struct csiphy_device *) sd->host_priv;
 573                         struct media_entity *input = &csiphy->subdev.entity;
 574                         unsigned int i;
 575
 576                         for (i = 0; i < sensor->num_pads; i++) {
 577                                 if (sensor->pads[i].flags & MEDIA_PAD_FL_SOURCE)
 578                                         break;
 579                         }
 580                         if (i == sensor->num_pads) {
 581                                 dev_err(camss->dev,
 582                                         "No source pad in external entity\n");
 583                                 return -EINVAL;
 584                         }
 585
 586                         ret = media_create_pad_link(sensor, i,
 587                                 input, MSM_CSIPHY_PAD_SINK,
 588                                 MEDIA_LNK_FL_IMMUTABLE | MEDIA_LNK_FL_ENABLED);
 589                         if (ret < 0) {
 590                                 dev_err(camss->dev,
 591                                         "Failed to link %s->%s entities: %d\n",
 592                                         sensor->name, input->name, ret);
 593                                 return ret;
 594                         }
 595                 }
 596         }
 597
 598         ret = v4l2_device_register_subdev_nodes(&camss->v4l2_dev);
 599         if (ret < 0)
 600                 return ret;
 601
 602         return media_device_register(&camss->media_dev);
 603 }
 604
 605 static const struct v4l2_async_notifier_operations camss_subdev_notifier_ops = {
 606         .bound = camss_subdev_notifier_bound,
 607         .complete = camss_subdev_notifier_complete,
 608 };
 609
 610 static const struct media_device_ops camss_media_ops = {
 611         .link_notify = v4l2_pipeline_link_notify,
 612 };
 613
 614 /*
 615  * camss_probe - Probe CAMSS platform device
 616  * @pdev: Pointer to CAMSS platform device
 617  *
 618  * Return 0 on success or a negative error code on failure
 619  */
 620 static int camss_probe(struct platform_device *pdev)
 621 {
 622         struct device *dev = &pdev->dev;
 623         struct camss *camss;
 624         int ret;
 625
 626         camss = kzalloc(sizeof(*camss), GFP_KERNEL);
 627         if (!camss)
 628                 return -ENOMEM;
 629
 630         atomic_set(&camss->ref_count, 0);
 631         camss->dev = dev;
 632         platform_set_drvdata(pdev, camss);
 633
 634         ret = camss_of_parse_ports(dev, &camss->notifier);
 635         if (ret < 0)
 636                 return ret;
 637
 638         ret = camss_init_subdevices(camss);
 639         if (ret < 0)
 640                 return ret;
 641
 642         ret = dma_set_mask_and_coherent(dev, 0xffffffff);
 643         if (ret)
 644                 return ret;
 645
 646         camss->media_dev.dev = camss->dev;
 647         strlcpy(camss->media_dev.model, "Qualcomm Camera Subsystem",
 648                 sizeof(camss->media_dev.model));
 649         camss->media_dev.ops = &camss_media_ops;
 650         media_device_init(&camss->media_dev);
 651
 652         camss->v4l2_dev.mdev = &camss->media_dev;
 653         ret = v4l2_device_register(camss->dev, &camss->v4l2_dev);
 654         if (ret < 0) {
 655                 dev_err(dev, "Failed to register V4L2 device: %d\n", ret);
 656                 return ret;
 657         }
 658
 659         ret = camss_register_entities(camss);
 660         if (ret < 0)
 661                 goto err_register_entities;
 662
 663         if (camss->notifier.num_subdevs) {
 664                 camss->notifier.ops = &camss_subdev_notifier_ops;
 665
 666                 ret = v4l2_async_notifier_register(&camss->v4l2_dev,
 667                                                    &camss->notifier);
 668                 if (ret) {
 669                         dev_err(dev,
 670                                 "Failed to register async subdev nodes: %d\n",
 671                                 ret);
 672                         goto err_register_subdevs;
 673                 }
 674         } else {
 675                 ret = v4l2_device_register_subdev_nodes(&camss->v4l2_dev);
 676                 if (ret < 0) {
 677                         dev_err(dev, "Failed to register subdev nodes: %d\n",
 678                                 ret);
 679                         goto err_register_subdevs;
 680                 }
 681
 682                 ret = media_device_register(&camss->media_dev);
 683                 if (ret < 0) {
 684                         dev_err(dev, "Failed to register media device: %d\n",
 685                                 ret);
 686                         goto err_register_subdevs;
 687                 }
 688         }
 689
 690         return 0;
 691
 692 err_register_subdevs:
 693         camss_unregister_entities(camss);
 694 err_register_entities:
 695         v4l2_device_unregister(&camss->v4l2_dev);
 696
 697         return ret;
 698 }
 699
 700 void camss_delete(struct camss *camss)
 701 {
 702         v4l2_device_unregister(&camss->v4l2_dev);
 703         media_device_unregister(&camss->media_dev);
 704         media_device_cleanup(&camss->media_dev);
 705
 706         kfree(camss);
 707 }
 708
 709 /*
 710  * camss_remove - Remove CAMSS platform device
 711  * @pdev: Pointer to CAMSS platform device
 712  *
 713  * Always returns 0.
 714  */
 715 static int camss_remove(struct platform_device *pdev)
 716 {
 717         struct camss *camss = platform_get_drvdata(pdev);
 718
 719         msm_vfe_stop_streaming(&camss->vfe);
 720
 721         v4l2_async_notifier_unregister(&camss->notifier);
 722         camss_unregister_entities(camss);
 723
 724         if (atomic_read(&camss->ref_count) == 0)
 725                 camss_delete(camss);
 726
 727         return 0;
 728 }
 729
 730 static const struct of_device_id camss_dt_match[] = {
 731         { .compatible = "qcom,msm8916-camss" },
 732         { }
 733 };
 734
 735 MODULE_DEVICE_TABLE(of, camss_dt_match);
 736
 737 static struct platform_driver qcom_camss_driver = {
 738         .probe = camss_probe,
 739         .remove = camss_remove,
 740         .driver = {
 741                 .name = "qcom-camss",
 742                 .of_match_table = camss_dt_match,
 743         },
 744 };
 745
 746 module_platform_driver(qcom_camss_driver);
 747
 748 MODULE_ALIAS("platform:qcom-camss");
 749 MODULE_DESCRIPTION("Qualcomm Camera Subsystem driver");
 750 MODULE_AUTHOR("Todor Tomov <todor.tomov@linaro.org>");
 751 MODULE_LICENSE("GPL v2");