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_kzalloc(dev, lncfg->num_data * sizeof(*lncfg->data),
 275                                    GFP_KERNEL);
 276         if (!lncfg->data)
 277                 return -ENOMEM;
 278
 279         for (i = 0; i < lncfg->num_data; i++) {
 280                 lncfg->data[i].pos = mipi_csi2->data_lanes[i];
 281                 lncfg->data[i].pol = mipi_csi2->lane_polarities[i + 1];
 282         }
 283
 284         return 0;
 285 }
 286
 287 /*
 288  * camss_of_parse_ports - Parse ports node
 289  * @dev: Device
 290  * @notifier: v4l2_device notifier data
 291  *
 292  * Return number of "port" nodes found in "ports" node
 293  */
 294 static int camss_of_parse_ports(struct device *dev,
 295                                 struct v4l2_async_notifier *notifier)
 296 {
 297         struct device_node *node = NULL;
 298         struct device_node *remote = NULL;
 299         unsigned int size, i;
 300         int ret;
 301
 302         while ((node = of_graph_get_next_endpoint(dev->of_node, node)))
 303                 if (of_device_is_available(node))
 304                         notifier->num_subdevs++;
 305
 306         size = sizeof(*notifier->subdevs) * notifier->num_subdevs;
 307         notifier->subdevs = devm_kzalloc(dev, size, GFP_KERNEL);
 308         if (!notifier->subdevs) {
 309                 dev_err(dev, "Failed to allocate memory\n");
 310                 return -ENOMEM;
 311         }
 312
 313         i = 0;
 314         while ((node = of_graph_get_next_endpoint(dev->of_node, node))) {
 315                 struct camss_async_subdev *csd;
 316
 317                 if (!of_device_is_available(node))
 318                         continue;
 319
 320                 csd = devm_kzalloc(dev, sizeof(*csd), GFP_KERNEL);
 321                 if (!csd) {
 322                         of_node_put(node);
 323                         dev_err(dev, "Failed to allocate memory\n");
 324                         return -ENOMEM;
 325                 }
 326
 327                 notifier->subdevs[i++] = &csd->asd;
 328
 329                 ret = camss_of_parse_endpoint_node(dev, node, csd);
 330                 if (ret < 0) {
 331                         of_node_put(node);
 332                         return ret;
 333                 }
 334
 335                 remote = of_graph_get_remote_port_parent(node);
 336                 of_node_put(node);
 337
 338                 if (!remote) {
 339                         dev_err(dev, "Cannot get remote parent\n");
 340                         return -EINVAL;
 341                 }
 342
 343                 csd->asd.match_type = V4L2_ASYNC_MATCH_FWNODE;
 344                 csd->asd.match.fwnode = of_fwnode_handle(remote);
 345         }
 346
 347         return notifier->num_subdevs;
 348 }
 349
 350 /*
 351  * camss_init_subdevices - Initialize subdev structures and resources
 352  * @camss: CAMSS device
 353  *
 354  * Return 0 on success or a negative error code on failure
 355  */
 356 static int camss_init_subdevices(struct camss *camss)
 357 {
 358         unsigned int i;
 359         int ret;
 360
 361         for (i = 0; i < ARRAY_SIZE(camss->csiphy); i++) {
 362                 ret = msm_csiphy_subdev_init(&camss->csiphy[i],
 363                                              &csiphy_res[i], i);
 364                 if (ret < 0) {
 365                         dev_err(camss->dev,
 366                                 "Failed to init csiphy%d sub-device: %d\n",
 367                                 i, ret);
 368                         return ret;
 369                 }
 370         }
 371
 372         for (i = 0; i < ARRAY_SIZE(camss->csid); i++) {
 373                 ret = msm_csid_subdev_init(&camss->csid[i],
 374                                            &csid_res[i], i);
 375                 if (ret < 0) {
 376                         dev_err(camss->dev,
 377                                 "Failed to init csid%d sub-device: %d\n",
 378                                 i, ret);
 379                         return ret;
 380                 }
 381         }
 382
 383         ret = msm_ispif_subdev_init(&camss->ispif, &ispif_res);
 384         if (ret < 0) {
 385                 dev_err(camss->dev, "Failed to init ispif sub-device: %d\n",
 386                         ret);
 387                 return ret;
 388         }
 389
 390         ret = msm_vfe_subdev_init(&camss->vfe, &vfe_res);
 391         if (ret < 0) {
 392                 dev_err(camss->dev, "Fail to init vfe sub-device: %d\n", ret);
 393                 return ret;
 394         }
 395
 396         return 0;
 397 }
 398
 399 /*
 400  * camss_register_entities - Register subdev nodes and create links
 401  * @camss: CAMSS device
 402  *
 403  * Return 0 on success or a negative error code on failure
 404  */
 405 static int camss_register_entities(struct camss *camss)
 406 {
 407         int i, j;
 408         int ret;
 409
 410         for (i = 0; i < ARRAY_SIZE(camss->csiphy); i++) {
 411                 ret = msm_csiphy_register_entity(&camss->csiphy[i],
 412                                                  &camss->v4l2_dev);
 413                 if (ret < 0) {
 414                         dev_err(camss->dev,
 415                                 "Failed to register csiphy%d entity: %d\n",
 416                                 i, ret);
 417                         goto err_reg_csiphy;
 418                 }
 419         }
 420
 421         for (i = 0; i < ARRAY_SIZE(camss->csid); i++) {
 422                 ret = msm_csid_register_entity(&camss->csid[i],
 423                                                &camss->v4l2_dev);
 424                 if (ret < 0) {
 425                         dev_err(camss->dev,
 426                                 "Failed to register csid%d entity: %d\n",
 427                                 i, ret);
 428                         goto err_reg_csid;
 429                 }
 430         }
 431
 432         ret = msm_ispif_register_entities(&camss->ispif, &camss->v4l2_dev);
 433         if (ret < 0) {
 434                 dev_err(camss->dev, "Failed to register ispif entities: %d\n",
 435                         ret);
 436                 goto err_reg_ispif;
 437         }
 438
 439         ret = msm_vfe_register_entities(&camss->vfe, &camss->v4l2_dev);
 440         if (ret < 0) {
 441                 dev_err(camss->dev, "Failed to register vfe entities: %d\n",
 442                         ret);
 443                 goto err_reg_vfe;
 444         }
 445
 446         for (i = 0; i < ARRAY_SIZE(camss->csiphy); i++) {
 447                 for (j = 0; j < ARRAY_SIZE(camss->csid); j++) {
 448                         ret = media_create_pad_link(
 449                                 &camss->csiphy[i].subdev.entity,
 450                                 MSM_CSIPHY_PAD_SRC,
 451                                 &camss->csid[j].subdev.entity,
 452                                 MSM_CSID_PAD_SINK,
 453                                 0);
 454                         if (ret < 0) {
 455                                 dev_err(camss->dev,
 456                                         "Failed to link %s->%s entities: %d\n",
 457                                         camss->csiphy[i].subdev.entity.name,
 458                                         camss->csid[j].subdev.entity.name,
 459                                         ret);
 460                                 goto err_link;
 461                         }
 462                 }
 463         }
 464
 465         for (i = 0; i < ARRAY_SIZE(camss->csid); i++) {
 466                 for (j = 0; j < ARRAY_SIZE(camss->ispif.line); j++) {
 467                         ret = media_create_pad_link(
 468                                 &camss->csid[i].subdev.entity,
 469                                 MSM_CSID_PAD_SRC,
 470                                 &camss->ispif.line[j].subdev.entity,
 471                                 MSM_ISPIF_PAD_SINK,
 472                                 0);
 473                         if (ret < 0) {
 474                                 dev_err(camss->dev,
 475                                         "Failed to link %s->%s entities: %d\n",
 476                                         camss->csid[i].subdev.entity.name,
 477                                         camss->ispif.line[j].subdev.entity.name,
 478                                         ret);
 479                                 goto err_link;
 480                         }
 481                 }
 482         }
 483
 484         for (i = 0; i < ARRAY_SIZE(camss->ispif.line); i++) {
 485                 for (j = 0; j < ARRAY_SIZE(camss->vfe.line); j++) {
 486                         ret = media_create_pad_link(
 487                                 &camss->ispif.line[i].subdev.entity,
 488                                 MSM_ISPIF_PAD_SRC,
 489                                 &camss->vfe.line[j].subdev.entity,
 490                                 MSM_VFE_PAD_SINK,
 491                                 0);
 492                         if (ret < 0) {
 493                                 dev_err(camss->dev,
 494                                         "Failed to link %s->%s entities: %d\n",
 495                                         camss->ispif.line[i].subdev.entity.name,
 496                                         camss->vfe.line[j].subdev.entity.name,
 497                                         ret);
 498                                 goto err_link;
 499                         }
 500                 }
 501         }
 502
 503         return 0;
 504
 505 err_link:
 506         msm_vfe_unregister_entities(&camss->vfe);
 507 err_reg_vfe:
 508         msm_ispif_unregister_entities(&camss->ispif);
 509 err_reg_ispif:
 510
 511         i = ARRAY_SIZE(camss->csid);
 512 err_reg_csid:
 513         for (i--; i >= 0; i--)
 514                 msm_csid_unregister_entity(&camss->csid[i]);
 515
 516         i = ARRAY_SIZE(camss->csiphy);
 517 err_reg_csiphy:
 518         for (i--; i >= 0; i--)
 519                 msm_csiphy_unregister_entity(&camss->csiphy[i]);
 520
 521         return ret;
 522 }
 523
 524 /*
 525  * camss_unregister_entities - Unregister subdev nodes
 526  * @camss: CAMSS device
 527  *
 528  * Return 0 on success or a negative error code on failure
 529  */
 530 static void camss_unregister_entities(struct camss *camss)
 531 {
 532         unsigned int i;
 533
 534         for (i = 0; i < ARRAY_SIZE(camss->csiphy); i++)
 535                 msm_csiphy_unregister_entity(&camss->csiphy[i]);
 536
 537         for (i = 0; i < ARRAY_SIZE(camss->csid); i++)
 538                 msm_csid_unregister_entity(&camss->csid[i]);
 539
 540         msm_ispif_unregister_entities(&camss->ispif);
 541         msm_vfe_unregister_entities(&camss->vfe);
 542 }
 543
 544 static int camss_subdev_notifier_bound(struct v4l2_async_notifier *async,
 545                                        struct v4l2_subdev *subdev,
 546                                        struct v4l2_async_subdev *asd)
 547 {
 548         struct camss *camss = container_of(async, struct camss, notifier);
 549         struct camss_async_subdev *csd =
 550                 container_of(asd, struct camss_async_subdev, asd);
 551         u8 id = csd->interface.csiphy_id;
 552         struct csiphy_device *csiphy = &camss->csiphy[id];
 553
 554         csiphy->cfg.csi2 = &csd->interface.csi2;
 555         subdev->host_priv = csiphy;
 556
 557         return 0;
 558 }
 559
 560 static int camss_subdev_notifier_complete(struct v4l2_async_notifier *async)
 561 {
 562         struct camss *camss = container_of(async, struct camss, notifier);
 563         struct v4l2_device *v4l2_dev = &camss->v4l2_dev;
 564         struct v4l2_subdev *sd;
 565         int ret;
 566
 567         list_for_each_entry(sd, &v4l2_dev->subdevs, list) {
 568                 if (sd->host_priv) {
 569                         struct media_entity *sensor = &sd->entity;
 570                         struct csiphy_device *csiphy =
 571                                         (struct csiphy_device *) sd->host_priv;
 572                         struct media_entity *input = &csiphy->subdev.entity;
 573                         unsigned int i;
 574
 575                         for (i = 0; i < sensor->num_pads; i++) {
 576                                 if (sensor->pads[i].flags & MEDIA_PAD_FL_SOURCE)
 577                                         break;
 578                         }
 579                         if (i == sensor->num_pads) {
 580                                 dev_err(camss->dev,
 581                                         "No source pad in external entity\n");
 582                                 return -EINVAL;
 583                         }
 584
 585                         ret = media_create_pad_link(sensor, i,
 586                                 input, MSM_CSIPHY_PAD_SINK,
 587                                 MEDIA_LNK_FL_IMMUTABLE | MEDIA_LNK_FL_ENABLED);
 588                         if (ret < 0) {
 589                                 dev_err(camss->dev,
 590                                         "Failed to link %s->%s entities: %d\n",
 591                                         sensor->name, input->name, ret);
 592                                 return ret;
 593                         }
 594                 }
 595         }
 596
 597         ret = v4l2_device_register_subdev_nodes(&camss->v4l2_dev);
 598         if (ret < 0)
 599                 return ret;
 600
 601         return media_device_register(&camss->media_dev);
 602 }
 603
 604 static const struct v4l2_async_notifier_operations camss_subdev_notifier_ops = {
 605         .bound = camss_subdev_notifier_bound,
 606         .complete = camss_subdev_notifier_complete,
 607 };
 608
 609 static const struct media_device_ops camss_media_ops = {
 610         .link_notify = v4l2_pipeline_link_notify,
 611 };
 612
 613 /*
 614  * camss_probe - Probe CAMSS platform device
 615  * @pdev: Pointer to CAMSS platform device
 616  *
 617  * Return 0 on success or a negative error code on failure
 618  */
 619 static int camss_probe(struct platform_device *pdev)
 620 {
 621         struct device *dev = &pdev->dev;
 622         struct camss *camss;
 623         int ret;
 624
 625         camss = kzalloc(sizeof(*camss), GFP_KERNEL);
 626         if (!camss)
 627                 return -ENOMEM;
 628
 629         atomic_set(&camss->ref_count, 0);
 630         camss->dev = dev;
 631         platform_set_drvdata(pdev, camss);
 632
 633         ret = camss_of_parse_ports(dev, &camss->notifier);
 634         if (ret < 0)
 635                 return ret;
 636
 637         ret = camss_init_subdevices(camss);
 638         if (ret < 0)
 639                 return ret;
 640
 641         ret = dma_set_mask_and_coherent(dev, 0xffffffff);
 642         if (ret)
 643                 return ret;
 644
 645         camss->media_dev.dev = camss->dev;
 646         strlcpy(camss->media_dev.model, "Qualcomm Camera Subsystem",
 647                 sizeof(camss->media_dev.model));
 648         camss->media_dev.ops = &camss_media_ops;
 649         media_device_init(&camss->media_dev);
 650
 651         camss->v4l2_dev.mdev = &camss->media_dev;
 652         ret = v4l2_device_register(camss->dev, &camss->v4l2_dev);
 653         if (ret < 0) {
 654                 dev_err(dev, "Failed to register V4L2 device: %d\n", ret);
 655                 return ret;
 656         }
 657
 658         ret = camss_register_entities(camss);
 659         if (ret < 0)
 660                 goto err_register_entities;
 661
 662         if (camss->notifier.num_subdevs) {
 663                 camss->notifier.ops = &camss_subdev_notifier_ops;
 664
 665                 ret = v4l2_async_notifier_register(&camss->v4l2_dev,
 666                                                    &camss->notifier);
 667                 if (ret) {
 668                         dev_err(dev,
 669                                 "Failed to register async subdev nodes: %d\n",
 670                                 ret);
 671                         goto err_register_subdevs;
 672                 }
 673         } else {
 674                 ret = v4l2_device_register_subdev_nodes(&camss->v4l2_dev);
 675                 if (ret < 0) {
 676                         dev_err(dev, "Failed to register subdev nodes: %d\n",
 677                                 ret);
 678                         goto err_register_subdevs;
 679                 }
 680
 681                 ret = media_device_register(&camss->media_dev);
 682                 if (ret < 0) {
 683                         dev_err(dev, "Failed to register media device: %d\n",
 684                                 ret);
 685                         goto err_register_subdevs;
 686                 }
 687         }
 688
 689         return 0;
 690
 691 err_register_subdevs:
 692         camss_unregister_entities(camss);
 693 err_register_entities:
 694         v4l2_device_unregister(&camss->v4l2_dev);
 695
 696         return ret;
 697 }
 698
 699 void camss_delete(struct camss *camss)
 700 {
 701         v4l2_device_unregister(&camss->v4l2_dev);
 702         media_device_unregister(&camss->media_dev);
 703         media_device_cleanup(&camss->media_dev);
 704
 705         kfree(camss);
 706 }
 707
 708 /*
 709  * camss_remove - Remove CAMSS platform device
 710  * @pdev: Pointer to CAMSS platform device
 711  *
 712  * Always returns 0.
 713  */
 714 static int camss_remove(struct platform_device *pdev)
 715 {
 716         struct camss *camss = platform_get_drvdata(pdev);
 717
 718         msm_vfe_stop_streaming(&camss->vfe);
 719
 720         v4l2_async_notifier_unregister(&camss->notifier);
 721         camss_unregister_entities(camss);
 722
 723         if (atomic_read(&camss->ref_count) == 0)
 724                 camss_delete(camss);
 725
 726         return 0;
 727 }
 728
 729 static const struct of_device_id camss_dt_match[] = {
 730         { .compatible = "qcom,msm8916-camss" },
 731         { }
 732 };
 733
 734 MODULE_DEVICE_TABLE(of, camss_dt_match);
 735
 736 static struct platform_driver qcom_camss_driver = {
 737         .probe = camss_probe,
 738         .remove = camss_remove,
 739         .driver = {
 740                 .name = "qcom-camss",
 741                 .of_match_table = camss_dt_match,
 742         },
 743 };
 744
 745 module_platform_driver(qcom_camss_driver);
 746
 747 MODULE_ALIAS("platform:qcom-camss");
 748 MODULE_DESCRIPTION("Qualcomm Camera Subsystem driver");
 749 MODULE_AUTHOR("Todor Tomov <todor.tomov@linaro.org>");
 750 MODULE_LICENSE("GPL v2");