drivers/media/platform/starfive/v4l2_driver/stfcamss.c

   1 // SPDX-License-Identifier: GPL-2.0
   2 /*
   3  * Copyright (C) 2021 StarFive Technology Co., Ltd.
   4  */
   5 #include <linux/completion.h>
   6 #include <linux/delay.h>
   7 #include <linux/dmaengine.h>
   8 #include <linux/init.h>
   9 #include <linux/interrupt.h>
  10 #include <linux/kernel.h>
  11 #include <linux/module.h>
  12 #include <linux/of.h>
  13 #include <linux/of_device.h>
  14 #include <linux/of_reserved_mem.h>
  15 #include <linux/of_graph.h>
  16 #include <linux/of_address.h>
  17 #include <linux/pinctrl/consumer.h>
  18 #include <linux/platform_device.h>
  19 #include <linux/pm_runtime.h>
  20 #include <linux/io.h>
  21 #include <linux/dma-mapping.h>
  22 #include <linux/uaccess.h>
  23 #include <linux/mfd/syscon.h>
  24
  25 #include <linux/videodev2.h>
  26
  27 #include <media/media-device.h>
  28 #include <media/v4l2-async.h>
  29 #include <media/v4l2-device.h>
  30 #include <media/v4l2-mc.h>
  31 #include <media/v4l2-fwnode.h>
  32 #include <linux/debugfs.h>
  33
  34 #include "stfcamss.h"
  35
  36 #ifdef STF_DEBUG
  37 unsigned int stdbg_level = ST_DEBUG;
  38 unsigned int stdbg_mask = 0x7F;
  39 #else
  40 unsigned int stdbg_level = ST_ERR;
  41 unsigned int stdbg_mask = 0x7F;
  42 #endif
  43 EXPORT_SYMBOL_GPL(stdbg_level);
  44 EXPORT_SYMBOL_GPL(stdbg_mask);
  45
  46 static const struct reg_name mem_reg_name[] = {
  47         {"csi2rx"},
  48         {"vclk"},
  49         {"vrst"},
  50         {"sctrl"},
  51         {"isp"},
  52         {"trst"},
  53         {"pmu"},
  54         {"syscrg"},
  55 };
  56
  57 static struct clk_bulk_data stfcamss_clocks[] = {
  58         { .id = "clk_apb_func" },
  59         { .id = "clk_pclk" },
  60         { .id = "clk_sys_clk" },
  61         { .id = "clk_wrapper_clk_c" },
  62         { .id = "clk_dvp_inv" },
  63         { .id = "clk_axiwr" },
  64         { .id = "clk_mipi_rx0_pxl" },
  65         { .id = "clk_pixel_clk_if0" },
  66         { .id = "clk_pixel_clk_if1" },
  67         { .id = "clk_pixel_clk_if2" },
  68         { .id = "clk_pixel_clk_if3" },
  69         { .id = "clk_m31dphy_cfgclk_in" },
  70         { .id = "clk_m31dphy_refclk_in" },
  71         { .id = "clk_m31dphy_txclkesc_lan0" },
  72         { .id = "clk_ispcore_2x" },
  73         { .id = "clk_isp_axi" },
  74         { .id = "clk_noc_bus_clk_isp_axi" },
  75 };
  76
  77 static struct reset_control_bulk_data stfcamss_resets[] = {
  78         { .id = "rst_wrapper_p" },
  79         { .id = "rst_wrapper_c" },
  80         { .id = "rst_pclk" },
  81         { .id = "rst_sys_clk" },
  82         { .id = "rst_axird" },
  83         { .id = "rst_axiwr" },
  84         { .id = "rst_pixel_clk_if0" },
  85         { .id = "rst_pixel_clk_if1" },
  86         { .id = "rst_pixel_clk_if2" },
  87         { .id = "rst_pixel_clk_if3" },
  88         { .id = "rst_m31dphy_hw" },
  89         { .id = "rst_m31dphy_b09_always_on" },
  90         { .id = "rst_isp_top_n" },
  91         { .id = "rst_isp_top_axi" },
  92 };
  93
  94 int stfcamss_get_mem_res(struct platform_device *pdev, struct stf_vin_dev *vin)
  95 {
  96         struct device *dev = &pdev->dev;
  97         struct resource *res;
  98         char *name;
  99         int i;
 100
 101         for (i = 0; i < ARRAY_SIZE(mem_reg_name); i++) {
 102                 name = (char *)(&mem_reg_name[i]);
 103                 res = platform_get_resource_byname(pdev, IORESOURCE_MEM, name);
 104
 105                 if (!res)
 106                         return -EINVAL;
 107
 108                 if (!strcmp(name, "csi2rx")) {
 109                         vin->csi2rx_base = devm_ioremap_resource(dev, res);
 110                         if (IS_ERR(vin->csi2rx_base))
 111                                 return PTR_ERR(vin->csi2rx_base);
 112                 } else if (!strcmp(name, "vclk")) {
 113                         vin->clkgen_base = ioremap(res->start, resource_size(res));
 114                         if (!vin->clkgen_base)
 115                                 return -ENOMEM;
 116                 } else if (!strcmp(name, "vrst")) {
 117                         vin->rstgen_base = devm_ioremap_resource(dev, res);
 118                         if (IS_ERR(vin->rstgen_base))
 119                                 return PTR_ERR(vin->rstgen_base);
 120                 } else if (!strcmp(name, "sctrl")) {
 121                         vin->sysctrl_base = devm_ioremap_resource(dev, res);
 122                         if (IS_ERR(vin->sysctrl_base))
 123                                 return PTR_ERR(vin->sysctrl_base);
 124                 } else if (!strcmp(name, "isp")) {
 125                         vin->isp_base = devm_ioremap_resource(dev, res);
 126                         if (IS_ERR(vin->isp_base))
 127                                 return PTR_ERR(vin->isp_base);
 128                 } else if (!strcmp(name, "trst")) {
 129                         vin->vin_top_rstgen_base = devm_ioremap_resource(dev, res);
 130                         if (IS_ERR(vin->vin_top_rstgen_base))
 131                                 return PTR_ERR(vin->vin_top_rstgen_base);
 132                 } else if (!strcmp(name, "pmu")) {
 133                         vin->pmu_test = ioremap(res->start, resource_size(res));
 134                         if (!vin->pmu_test)
 135                                 return -ENOMEM;
 136                 } else if (!strcmp(name, "syscrg")) {
 137                         vin->sys_crg = ioremap(res->start, resource_size(res));
 138                         if (!vin->sys_crg)
 139                                 return -ENOMEM;
 140                 } else {
 141                         st_err(ST_CAMSS, "Could not match resource name\n");
 142                 }
 143         }
 144
 145         return 0;
 146 }
 147
 148 int vin_parse_dt(struct device *dev, struct stf_vin_dev *vin)
 149 {
 150         int ret = 0;
 151         struct device_node *np = dev->of_node;
 152
 153         if (!np)
 154                 return -EINVAL;
 155
 156         return ret;
 157 }
 158
 159 struct media_entity *stfcamss_find_sensor(struct media_entity *entity)
 160 {
 161         struct media_pad *pad;
 162
 163         while (1) {
 164                 if (!entity->pads)
 165                         return NULL;
 166
 167                 pad = &entity->pads[0];
 168                 if (!(pad->flags & MEDIA_PAD_FL_SINK))
 169                         return NULL;
 170
 171                 pad = media_entity_remote_pad(pad);
 172                 if (!pad || !is_media_entity_v4l2_subdev(pad->entity))
 173                         return NULL;
 174
 175                 entity = pad->entity;
 176
 177                 if (entity->function == MEDIA_ENT_F_CAM_SENSOR)
 178                         return entity;
 179         }
 180 }
 181
 182 static int stfcamss_of_parse_endpoint_node(struct device *dev,
 183                                 struct device_node *node,
 184                                 struct stfcamss_async_subdev *csd)
 185 {
 186         struct v4l2_fwnode_endpoint vep = { { 0 } };
 187         struct v4l2_fwnode_bus_parallel *parallel_bus = &vep.bus.parallel;
 188         struct v4l2_fwnode_bus_mipi_csi2 *csi2_bus = &vep.bus.mipi_csi2;
 189         struct dvp_cfg *dvp = &csd->interface.dvp;
 190         struct csi2phy_cfg *csiphy = &csd->interface.csiphy;
 191
 192         v4l2_fwnode_endpoint_parse(of_fwnode_handle(node), &vep);
 193         st_debug(ST_CAMSS, "%s: vep.base.port = 0x%x, id = 0x%x\n",
 194                         __func__, vep.base.port, vep.base.id);
 195
 196         csd->port = vep.base.port;
 197         switch (csd->port) {
 198         case DVP_SENSOR_PORT_NUMBER:
 199                 st_debug(ST_CAMSS, "%s, flags = 0x%x\n", __func__,
 200                                 parallel_bus->flags);
 201                 dvp->flags = parallel_bus->flags;
 202                 dvp->bus_width = parallel_bus->bus_width;
 203                 dvp->data_shift = parallel_bus->data_shift;
 204                 break;
 205         case CSI2RX_SENSOR_PORT_NUMBER:
 206                 st_debug(ST_CAMSS, "%s, CSI2 flags = 0x%x\n",
 207                                 __func__, parallel_bus->flags);
 208                 csiphy->flags = csi2_bus->flags;
 209                 memcpy(csiphy->data_lanes,
 210                                 csi2_bus->data_lanes, csi2_bus->num_data_lanes);
 211                 csiphy->clock_lane = csi2_bus->clock_lane;
 212                 csiphy->num_data_lanes = csi2_bus->num_data_lanes;
 213                 memcpy(csiphy->lane_polarities,
 214                                 csi2_bus->lane_polarities,
 215                                 csi2_bus->num_data_lanes + 1);
 216                 break;
 217         default:
 218                 break;
 219         };
 220
 221         return 0;
 222 }
 223
 224 static int stfcamss_of_parse_ports(struct stfcamss *stfcamss)
 225 {
 226         struct device *dev = stfcamss->dev;
 227         struct device_node *node = NULL;
 228         struct device_node *remote = NULL;
 229         int ret, num_subdevs = 0;
 230
 231         for_each_endpoint_of_node(dev->of_node, node) {
 232                 struct stfcamss_async_subdev *csd;
 233
 234                 if (!of_device_is_available(node))
 235                         continue;
 236
 237                 remote = of_graph_get_remote_port_parent(node);
 238                 if (!remote) {
 239                         st_err(ST_CAMSS, "Cannot get remote parent\n");
 240                         ret = -EINVAL;
 241                         goto err_cleanup;
 242                 }
 243
 244                 csd = v4l2_async_notifier_add_fwnode_subdev(
 245                         &stfcamss->notifier, of_fwnode_handle(remote),
 246                         struct stfcamss_async_subdev);
 247                 of_node_put(remote);
 248                 if (IS_ERR(csd)) {
 249                         ret = PTR_ERR(csd);
 250                         goto err_cleanup;
 251                 }
 252
 253                 ret = stfcamss_of_parse_endpoint_node(dev, node, csd);
 254                 if (ret < 0)
 255                         goto err_cleanup;
 256
 257                 num_subdevs++;
 258         }
 259
 260         return num_subdevs;
 261
 262 err_cleanup:
 263         of_node_put(node);
 264         return ret;
 265 }
 266
 267 static int stfcamss_init_subdevices(struct stfcamss *stfcamss)
 268 {
 269         int ret;
 270
 271         ret = stf_dvp_subdev_init(stfcamss);
 272         if (ret < 0) {
 273                 st_err(ST_CAMSS,
 274                         "Failed to init stf_dvp sub-device: %d\n",
 275                         ret);
 276                 return ret;
 277         }
 278
 279         ret = stf_csiphy_subdev_init(stfcamss);
 280         if (ret < 0) {
 281                 st_err(ST_CAMSS,
 282                         "Failed to init stf_csiphy sub-device: %d\n",
 283                         ret);
 284                 return ret;
 285         }
 286
 287         ret = stf_csi_subdev_init(stfcamss);
 288         if (ret < 0) {
 289                 st_err(ST_CAMSS,
 290                         "Failed to init stf_csi sub-device: %d\n",
 291                         ret);
 292                 return ret;
 293         }
 294
 295         ret = stf_isp_subdev_init(stfcamss);
 296         if (ret < 0) {
 297                 st_err(ST_CAMSS,
 298                         "Failed to init stf_isp sub-device: %d\n",
 299                         ret);
 300                 return ret;
 301         }
 302
 303         ret = stf_vin_subdev_init(stfcamss);
 304         if (ret < 0) {
 305                 st_err(ST_CAMSS,
 306                         "Failed to init stf_vin sub-device: %d\n",
 307                         ret);
 308                 return ret;
 309         }
 310         return ret;
 311 }
 312
 313 static int stfcamss_register_subdevices(struct stfcamss *stfcamss)
 314 {
 315         int ret;
 316         struct stf_vin2_dev *vin_dev = stfcamss->vin_dev;
 317         struct stf_dvp_dev *dvp_dev = stfcamss->dvp_dev;
 318         struct stf_csiphy_dev *csiphy_dev = stfcamss->csiphy_dev;
 319         struct stf_csi_dev *csi_dev = stfcamss->csi_dev;
 320         struct stf_isp_dev *isp_dev = stfcamss->isp_dev;
 321
 322         ret = stf_dvp_register(dvp_dev, &stfcamss->v4l2_dev);
 323         if (ret < 0) {
 324                 st_err(ST_CAMSS,
 325                         "Failed to register stf dvp%d entity: %d\n",
 326                         0, ret);
 327                 goto err_reg_dvp;
 328         }
 329
 330         ret = stf_csiphy_register(csiphy_dev, &stfcamss->v4l2_dev);
 331         if (ret < 0) {
 332                 st_err(ST_CAMSS,
 333                         "Failed to register stf csiphy%d entity: %d\n",
 334                         0, ret);
 335                 goto err_reg_csiphy;
 336         }
 337
 338         ret = stf_csi_register(csi_dev, &stfcamss->v4l2_dev);
 339         if (ret < 0) {
 340                 st_err(ST_CAMSS,
 341                         "Failed to register stf csi%d entity: %d\n",
 342                         0, ret);
 343                 goto err_reg_csi;
 344         }
 345
 346         ret = stf_isp_register(isp_dev, &stfcamss->v4l2_dev);
 347         if (ret < 0) {
 348                 st_err(ST_CAMSS,
 349                         "Failed to register stf isp%d entity: %d\n",
 350                         0, ret);
 351                 goto err_reg_isp;
 352         }
 353
 354         ret = stf_vin_register(vin_dev, &stfcamss->v4l2_dev);
 355         if (ret < 0) {
 356                 st_err(ST_CAMSS,
 357                         "Failed to register vin entity: %d\n",
 358                          ret);
 359                 goto err_reg_vin;
 360         }
 361
 362         ret = media_create_pad_link(
 363                 &dvp_dev->subdev.entity,
 364                 STF_DVP_PAD_SRC,
 365                 &vin_dev->line[VIN_LINE_WR].subdev.entity,
 366                 STF_VIN_PAD_SINK,
 367                 0);
 368         if (ret < 0) {
 369                 st_err(ST_CAMSS,
 370                         "Failed to link %s->vin entities: %d\n",
 371                         dvp_dev->subdev.entity.name,
 372                         ret);
 373                 goto err_link;
 374         }
 375
 376         ret = media_create_pad_link(
 377                 &csi_dev->subdev.entity,
 378                 STF_CSI_PAD_SRC,
 379                 &vin_dev->line[VIN_LINE_WR].subdev.entity,
 380                 STF_VIN_PAD_SINK,
 381                 0);
 382         if (ret < 0) {
 383                 st_err(ST_CAMSS,
 384                         "Failed to link %s->vin entities: %d\n",
 385                         csi_dev->subdev.entity.name,
 386                         ret);
 387                 goto err_link;
 388         }
 389
 390         ret = media_create_pad_link(
 391                 &csiphy_dev->subdev.entity,
 392                 STF_CSIPHY_PAD_SRC,
 393                 &csi_dev->subdev.entity,
 394                 STF_CSI_PAD_SINK,
 395                 MEDIA_LNK_FL_IMMUTABLE | MEDIA_LNK_FL_ENABLED);
 396         if (ret < 0) {
 397                 st_err(ST_CAMSS,
 398                         "Failed to link %s->%s entities: %d\n",
 399                         csiphy_dev->subdev.entity.name,
 400                         csi_dev->subdev.entity.name,
 401                         ret);
 402                 goto err_link;
 403         }
 404
 405         ret = media_create_pad_link(
 406                 &isp_dev->subdev.entity,
 407                 STF_ISP_PAD_SRC,
 408                 &vin_dev->line[VIN_LINE_ISP].subdev.entity,
 409                 STF_VIN_PAD_SINK,
 410                 0);
 411         if (ret < 0) {
 412                 st_err(ST_CAMSS,
 413                         "Failed to link %s->%s entities: %d\n",
 414                         isp_dev->subdev.entity.name,
 415                         vin_dev->line[VIN_LINE_ISP]
 416                         .subdev.entity.name,
 417                         ret);
 418                 goto err_link;
 419         }
 420
 421         ret = media_create_pad_link(
 422                 &isp_dev->subdev.entity,
 423                 STF_ISP_PAD_SRC_SS0,
 424                 &vin_dev->line[VIN_LINE_ISP_SS0].subdev.entity,
 425                 STF_VIN_PAD_SINK,
 426                 0);
 427         if (ret < 0) {
 428                 st_err(ST_CAMSS,
 429                         "Failed to link %s->%s entities: %d\n",
 430                         isp_dev->subdev.entity.name,
 431                         vin_dev->line[VIN_LINE_ISP_SS0]
 432                         .subdev.entity.name,
 433                         ret);
 434                 goto err_link;
 435         }
 436
 437         ret = media_create_pad_link(
 438                 &isp_dev->subdev.entity,
 439                 STF_ISP_PAD_SRC_SS1,
 440                 &vin_dev->line[VIN_LINE_ISP_SS1].subdev.entity,
 441                 STF_VIN_PAD_SINK,
 442                 0);
 443         if (ret < 0) {
 444                 st_err(ST_CAMSS,
 445                         "Failed to link %s->%s entities: %d\n",
 446                         isp_dev->subdev.entity.name,
 447                         vin_dev->line[VIN_LINE_ISP_SS1]
 448                         .subdev.entity.name,
 449                         ret);
 450                 goto err_link;
 451         }
 452
 453 #ifndef STF_CAMSS_SKIP_ITI
 454         ret = media_create_pad_link(
 455                 &isp_dev->subdev.entity,
 456                 STF_ISP_PAD_SRC_ITIW,
 457                 &vin_dev->line[VIN_LINE_ISP_ITIW].subdev.entity,
 458                 STF_VIN_PAD_SINK,
 459                 0);
 460         if (ret < 0) {
 461                 st_err(ST_CAMSS,
 462                         "Failed to link %s->%s entities: %d\n",
 463                         isp_dev->subdev.entity.name,
 464                         vin_dev->line[VIN_LINE_ISP_ITIW]
 465                         .subdev.entity.name,
 466                         ret);
 467                 goto err_link;
 468         }
 469
 470         ret = media_create_pad_link(
 471                 &isp_dev->subdev.entity,
 472                 STF_ISP_PAD_SRC_ITIR,
 473                 &vin_dev->line[VIN_LINE_ISP_ITIR].subdev.entity,
 474                 STF_VIN_PAD_SINK,
 475                 0);
 476         if (ret < 0) {
 477                 st_err(ST_CAMSS,
 478                         "Failed to link %s->%s entities: %d\n",
 479                         isp_dev->subdev.entity.name,
 480                         vin_dev->line[VIN_LINE_ISP_ITIR]
 481                         .subdev.entity.name,
 482                         ret);
 483                 goto err_link;
 484         }
 485 #endif
 486
 487         ret = media_create_pad_link(
 488                 &isp_dev->subdev.entity,
 489                 STF_ISP_PAD_SRC_RAW,
 490                 &vin_dev->line[VIN_LINE_ISP_RAW].subdev.entity,
 491                 STF_VIN_PAD_SINK,
 492                 0);
 493         if (ret < 0) {
 494                 st_err(ST_CAMSS,
 495                         "Failed to link %s->%s entities: %d\n",
 496                         isp_dev->subdev.entity.name,
 497                         vin_dev->line[VIN_LINE_ISP_RAW]
 498                         .subdev.entity.name,
 499                         ret);
 500                 goto err_link;
 501         }
 502
 503         ret = media_create_pad_link(
 504                 &isp_dev->subdev.entity,
 505                 STF_ISP_PAD_SRC_SCD_Y,
 506                 &vin_dev->line[VIN_LINE_ISP_SCD_Y].subdev.entity,
 507                 STF_VIN_PAD_SINK,
 508                 0);
 509         if (ret < 0) {
 510                 st_err(ST_CAMSS,
 511                         "Failed to link %s->%s entities: %d\n",
 512                         isp_dev->subdev.entity.name,
 513                         vin_dev->line[VIN_LINE_ISP_SCD_Y]
 514                         .subdev.entity.name,
 515                         ret);
 516                 goto err_link;
 517         }
 518
 519         ret = media_create_pad_link(
 520                 &dvp_dev->subdev.entity,
 521                 STF_DVP_PAD_SRC,
 522                 &isp_dev->subdev.entity,
 523                 STF_ISP_PAD_SINK,
 524                 0);
 525         if (ret < 0) {
 526                 st_err(ST_CAMSS,
 527                         "Failed to link %s->%s entities: %d\n",
 528                         dvp_dev->subdev.entity.name,
 529                         isp_dev->subdev.entity.name,
 530                 ret);
 531                 goto err_link;
 532         }
 533
 534         ret = media_create_pad_link(
 535                 &csi_dev->subdev.entity,
 536                 STF_CSI_PAD_SRC,
 537                 &isp_dev->subdev.entity,
 538                 STF_ISP_PAD_SINK,
 539                 0);
 540         if (ret < 0) {
 541                 st_err(ST_CAMSS,
 542                         "Failed to link %s->%s entities: %d\n",
 543                         csi_dev->subdev.entity.name,
 544                         isp_dev->subdev.entity.name,
 545                         ret);
 546                 goto err_link;
 547         }
 548
 549         return ret;
 550
 551 err_link:
 552         stf_vin_unregister(stfcamss->vin_dev);
 553 err_reg_vin:
 554         stf_isp_unregister(stfcamss->isp_dev);
 555 err_reg_isp:
 556         stf_csi_unregister(stfcamss->csi_dev);
 557 err_reg_csi:
 558         stf_csiphy_unregister(stfcamss->csiphy_dev);
 559 err_reg_csiphy:
 560         stf_dvp_unregister(stfcamss->dvp_dev);
 561 err_reg_dvp:
 562         return ret;
 563 }
 564
 565 static void stfcamss_unregister_subdevices(struct stfcamss *stfcamss)
 566 {
 567         stf_dvp_unregister(stfcamss->dvp_dev);
 568         stf_csiphy_unregister(stfcamss->csiphy_dev);
 569         stf_csi_unregister(stfcamss->csi_dev);
 570         stf_isp_unregister(stfcamss->isp_dev);
 571         stf_vin_unregister(stfcamss->vin_dev);
 572 }
 573
 574 static int stfcamss_register_mediadevice_subdevnodes(
 575                 struct v4l2_async_notifier *async,
 576                 struct v4l2_subdev *sd)
 577 {
 578         struct stfcamss *stfcamss =
 579                 container_of(async, struct stfcamss, notifier);
 580         int ret;
 581
 582         if (sd->host_priv) {
 583                 struct media_entity *sensor = &sd->entity;
 584                 struct media_entity *input = sd->host_priv;
 585                 unsigned int i;
 586
 587                 for (i = 0; i < sensor->num_pads; i++) {
 588                         if (sensor->pads[i].flags & MEDIA_PAD_FL_SOURCE)
 589                                 break;
 590                 }
 591                 if (i == sensor->num_pads) {
 592                         st_err(ST_CAMSS,
 593                                 "No source pad in external entity\n");
 594                         return -EINVAL;
 595                 }
 596
 597                 ret = media_create_pad_link(sensor, i,
 598                         input, STF_PAD_SINK,
 599                         MEDIA_LNK_FL_IMMUTABLE | MEDIA_LNK_FL_ENABLED);
 600                 if (ret < 0) {
 601                         st_err(ST_CAMSS,
 602                                 "Failed to link %s->%s entities: %d\n",
 603                                 sensor->name, input->name, ret);
 604                         return ret;
 605                 }
 606         }
 607
 608         ret = v4l2_device_register_subdev_nodes(&stfcamss->v4l2_dev);
 609         if (ret < 0)
 610                 return ret;
 611
 612         if (stfcamss->media_dev.devnode)
 613                 return ret;
 614
 615         st_debug(ST_CAMSS, "stfcamss register media device\n");
 616         return media_device_register(&stfcamss->media_dev);
 617 }
 618
 619 static int stfcamss_subdev_notifier_bound(struct v4l2_async_notifier *async,
 620                                         struct v4l2_subdev *subdev,
 621                                         struct v4l2_async_subdev *asd)
 622 {
 623         struct stfcamss *stfcamss =
 624                 container_of(async, struct stfcamss, notifier);
 625         struct stfcamss_async_subdev *csd =
 626                 container_of(asd, struct stfcamss_async_subdev, asd);
 627         enum port_num port = csd->port;
 628         struct stf_dvp_dev *dvp_dev = stfcamss->dvp_dev;
 629         struct stf_csiphy_dev *csiphy_dev = stfcamss->csiphy_dev;
 630
 631         switch (port) {
 632         case DVP_SENSOR_PORT_NUMBER:
 633                 dvp_dev->dvp = &csd->interface.dvp;
 634                 subdev->host_priv = &dvp_dev->subdev.entity;
 635                 break;
 636         case CSI2RX_SENSOR_PORT_NUMBER:
 637                 csiphy_dev->csiphy = &csd->interface.csiphy;
 638                 subdev->host_priv = &csiphy_dev->subdev.entity;
 639                 break;
 640         default:
 641                 break;
 642         };
 643
 644         stfcamss_register_mediadevice_subdevnodes(async, subdev);
 645
 646         return 0;
 647 }
 648
 649 #ifdef UNUSED_CODE
 650 static int stfcamss_subdev_notifier_complete(
 651                 struct v4l2_async_notifier *async)
 652 {
 653         struct stfcamss *stfcamss =
 654                 container_of(async, struct stfcamss, notifier);
 655         struct v4l2_device *v4l2_dev = &stfcamss->v4l2_dev;
 656         struct v4l2_subdev *sd;
 657         int ret;
 658
 659         list_for_each_entry(sd, &v4l2_dev->subdevs, list) {
 660                 if (sd->host_priv) {
 661                         struct media_entity *sensor = &sd->entity;
 662                         struct media_entity *input = sd->host_priv;
 663                         unsigned int i;
 664
 665                         for (i = 0; i < sensor->num_pads; i++) {
 666                                 if (sensor->pads[i].flags & MEDIA_PAD_FL_SOURCE)
 667                                         break;
 668                         }
 669                         if (i == sensor->num_pads) {
 670                                 st_err(ST_CAMSS,
 671                                         "No source pad in external entity\n");
 672                                 return -EINVAL;
 673                         }
 674
 675                         ret = media_create_pad_link(sensor, i,
 676                                 input, STF_PAD_SINK,
 677                                 MEDIA_LNK_FL_IMMUTABLE | MEDIA_LNK_FL_ENABLED);
 678                         if (ret < 0) {
 679                                 st_err(ST_CAMSS,
 680                                         "Failed to link %s->%s entities: %d\n",
 681                                         sensor->name, input->name, ret);
 682                                 return ret;
 683                         }
 684                 }
 685         }
 686
 687         ret = v4l2_device_register_subdev_nodes(&stfcamss->v4l2_dev);
 688         if (ret < 0)
 689                 return ret;
 690
 691         return media_device_register(&stfcamss->media_dev);
 692 }
 693 #endif
 694
 695 static const struct v4l2_async_notifier_operations
 696 stfcamss_subdev_notifier_ops = {
 697         .bound = stfcamss_subdev_notifier_bound,
 698 };
 699
 700 static const struct media_device_ops stfcamss_media_ops = {
 701         .link_notify = v4l2_pipeline_link_notify,
 702 };
 703
 704 #ifdef CONFIG_DEBUG_FS
 705 enum module_id {
 706         VIN_MODULE = 0,
 707         ISP_MODULE,
 708         CSI_MODULE,
 709         CSIPHY_MODULE,
 710         DVP_MODULE,
 711         CLK_MODULE,
 712 };
 713
 714 static enum module_id id_num = ISP_MODULE;
 715
 716 void dump_clk_reg(void __iomem *reg_base)
 717 {
 718         int i;
 719
 720         st_info(ST_CAMSS, "DUMP Clk register:\n");
 721         for (i = 0; i <= CLK_C_ISP_CTRL; i += 4)
 722                 print_reg(ST_CAMSS, reg_base, i);
 723 }
 724
 725 static ssize_t vin_debug_read(struct file *file, char __user *user_buf,
 726                         size_t count, loff_t *ppos)
 727 {
 728         struct device *dev = file->private_data;
 729         void __iomem *reg_base;
 730         struct stfcamss *stfcamss = dev_get_drvdata(dev);
 731         struct stf_vin_dev *vin = stfcamss->vin;
 732         struct stf_vin2_dev *vin_dev = stfcamss->vin_dev;
 733         struct stf_isp_dev *isp_dev = stfcamss->isp_dev;
 734         struct stf_csi_dev *csi0_dev = stfcamss->csi_dev;
 735
 736         switch (id_num) {
 737         case VIN_MODULE:
 738         case CSIPHY_MODULE:
 739         case DVP_MODULE:
 740                 mutex_lock(&vin_dev->power_lock);
 741                 if (vin_dev->power_count > 0) {
 742                         reg_base = vin->sysctrl_base;
 743                         dump_vin_reg(reg_base);
 744                 }
 745                 mutex_unlock(&vin_dev->power_lock);
 746                 break;
 747         case ISP_MODULE:
 748                 mutex_lock(&isp_dev->stream_lock);
 749                 if (isp_dev->stream_count > 0) {
 750                         reg_base = vin->isp_base;
 751                         dump_isp_reg(reg_base);
 752                 }
 753                 mutex_unlock(&isp_dev->stream_lock);
 754                 break;
 755         case CSI_MODULE:
 756                 mutex_lock(&csi0_dev->stream_lock);
 757                 if (csi0_dev->stream_count > 0) {
 758                         reg_base = vin->csi2rx_base;
 759                         dump_csi_reg(reg_base);
 760                 }
 761                 mutex_unlock(&csi0_dev->stream_lock);
 762                 break;
 763         case CLK_MODULE:
 764                 mutex_lock(&vin_dev->power_lock);
 765                 if (vin_dev->power_count > 0) {
 766                         reg_base = vin->clkgen_base;
 767                         dump_clk_reg(reg_base);
 768                 }
 769                 mutex_unlock(&vin_dev->power_lock);
 770                 break;
 771         default:
 772                 break;
 773         }
 774
 775         return 0;
 776 }
 777
 778 static void set_reg_val(struct stfcamss *stfcamss, int id, u32 offset, u32 val)
 779 {
 780         struct stf_vin_dev *vin = stfcamss->vin;
 781         struct stf_vin2_dev *vin_dev = stfcamss->vin_dev;
 782         struct stf_isp_dev *isp_dev = stfcamss->isp_dev;
 783         struct stf_csi_dev *csi_dev = stfcamss->csi_dev;
 784         void __iomem *reg_base;
 785
 786         switch (id) {
 787         case VIN_MODULE:
 788         case CSIPHY_MODULE:
 789         case DVP_MODULE:
 790                 mutex_lock(&vin_dev->power_lock);
 791                 if (vin_dev->power_count > 0) {
 792                         reg_base = vin->sysctrl_base;
 793                         print_reg(ST_VIN, reg_base, offset);
 794                         reg_write(reg_base, offset, val);
 795                         print_reg(ST_VIN, reg_base, offset);
 796                 }
 797                 mutex_unlock(&vin_dev->power_lock);
 798                 break;
 799         case ISP_MODULE:
 800                 mutex_lock(&isp_dev->stream_lock);
 801                 if (isp_dev->stream_count > 0) {
 802                         reg_base = vin->isp_base;
 803                         print_reg(ST_ISP, reg_base, offset);
 804                         reg_write(reg_base, offset, val);
 805                         print_reg(ST_ISP, reg_base, offset);
 806                 }
 807                 mutex_unlock(&isp_dev->stream_lock);
 808                 break;
 809         case CSI_MODULE:
 810                 mutex_lock(&csi_dev->stream_lock);
 811                 if (csi_dev->stream_count > 0) {
 812                         reg_base = vin->csi2rx_base;
 813                         print_reg(ST_CSI, reg_base, offset);
 814                         reg_write(reg_base, offset, val);
 815                         print_reg(ST_CSI, reg_base, offset);
 816                 }
 817                 mutex_unlock(&csi_dev->stream_lock);
 818                 break;
 819         case CLK_MODULE:
 820                 mutex_lock(&vin_dev->power_lock);
 821                 if (vin_dev->power_count > 0) {
 822                         reg_base = vin->clkgen_base;
 823                         print_reg(ST_CAMSS, reg_base, offset);
 824                         reg_write(reg_base, offset, val);
 825                         print_reg(ST_CAMSS, reg_base, offset);
 826                 }
 827                 mutex_unlock(&vin_dev->power_lock);
 828                 break;
 829         default:
 830                 break;
 831
 832         }
 833 }
 834
 835 static u32 atoi(const char *s)
 836 {
 837         u32 ret = 0, d = 0;
 838         char ch;
 839         int hex = 0;
 840
 841         if ((*s == '0') && (*(s+1) == 'x')) {
 842                 hex = 1;
 843                 s += 2;
 844         }
 845
 846         while (1) {
 847                 if (!hex) {
 848                         d = (*s++) - '0';
 849                         if (d > 9)
 850                                 break;
 851                         ret *= 10;
 852                         ret += d;
 853                 } else {
 854                         ch = tolower(*s++);
 855                         if (isdigit(ch))
 856                                 d = ch - '0';
 857                         else if (islower(ch))
 858                                 d = ch - 'a' + 10;
 859                         else
 860                                 break;
 861                         if (d > 15)
 862                                 break;
 863                         ret *= 16;
 864                         ret += d;
 865                 }
 866         }
 867
 868         return ret;
 869 }
 870
 871 static ssize_t vin_debug_write(struct file *file, const char __user *user_buf,
 872                                 size_t count, loff_t *ppos)
 873 {
 874         struct device *dev = file->private_data;
 875         struct stfcamss *stfcamss = dev_get_drvdata(dev);
 876         char *buf;
 877         char *line;
 878         char *p;
 879         static const char *delims = " \t\r";
 880         char *token;
 881         u32 offset, val;
 882
 883         buf = memdup_user_nul(user_buf, min_t(size_t, PAGE_SIZE, count));
 884         if (IS_ERR(buf))
 885                 return PTR_ERR(buf);
 886         p = buf;
 887         st_debug(ST_CAMSS, "dup buf: %s, len: %lu, count: %lu\n", p, strlen(p), count);
 888         while (p && *p) {
 889                 p = skip_spaces(p);
 890                 line = strsep(&p, "\n");
 891                 if (!*line || *line == '#')
 892                         break;
 893                 token = strsep(&line, delims);
 894                 if (!token)
 895                         goto out;
 896                 id_num = atoi(token);
 897                 token = strsep(&line, delims);
 898                 if (!token)
 899                         goto out;
 900                 offset = atoi(token);
 901                 token = strsep(&line, delims);
 902                 if (!token)
 903                         goto out;
 904                 val = atoi(token);
 905         }
 906         set_reg_val(stfcamss, id_num, offset, val);
 907 out:
 908         kfree(buf);
 909         st_info(ST_CAMSS, "id_num = %d, offset = 0x%x, 0x%x\n", id_num, offset, val);
 910         return count;
 911 }
 912
 913 static const struct file_operations vin_debug_fops = {
 914         .open = simple_open,
 915         .read = vin_debug_read,
 916         .write = vin_debug_write,
 917 };
 918 #endif /* CONFIG_DEBUG_FS */
 919
 920
 921 static int stfcamss_probe(struct platform_device *pdev)
 922 {
 923         struct stfcamss *stfcamss;
 924         struct stf_vin_dev *vin;
 925         struct device *dev = &pdev->dev;
 926         struct of_phandle_args args;
 927         int ret = 0, num_subdevs;
 928
 929         dev_info(dev, "stfcamss probe enter!\n");
 930
 931         stfcamss = devm_kzalloc(dev, sizeof(struct stfcamss), GFP_KERNEL);
 932         if (!stfcamss)
 933                 return -ENOMEM;
 934
 935         stfcamss->dvp_dev = devm_kzalloc(dev,
 936                 sizeof(*stfcamss->dvp_dev), GFP_KERNEL);
 937         if (!stfcamss->dvp_dev) {
 938                 ret = -ENOMEM;
 939                 goto err_cam;
 940         }
 941
 942         stfcamss->csiphy_dev = devm_kzalloc(dev,
 943                 sizeof(*stfcamss->csiphy_dev),
 944                 GFP_KERNEL);
 945         if (!stfcamss->csiphy_dev) {
 946                 ret = -ENOMEM;
 947                 goto err_cam;
 948         }
 949
 950         stfcamss->csi_dev = devm_kzalloc(dev,
 951                 sizeof(*stfcamss->csi_dev),
 952                 GFP_KERNEL);
 953         if (!stfcamss->csi_dev) {
 954                 ret = -ENOMEM;
 955                 goto err_cam;
 956         }
 957
 958         stfcamss->isp_dev = devm_kzalloc(dev,
 959                 sizeof(*stfcamss->isp_dev),
 960                 GFP_KERNEL);
 961         if (!stfcamss->isp_dev) {
 962                 ret = -ENOMEM;
 963                 goto err_cam;
 964         }
 965
 966         stfcamss->vin_dev = devm_kzalloc(dev,
 967                 sizeof(*stfcamss->vin_dev),
 968                 GFP_KERNEL);
 969         if (!stfcamss->vin_dev) {
 970                 ret = -ENOMEM;
 971                 goto err_cam;
 972         }
 973
 974         stfcamss->vin = devm_kzalloc(dev,
 975                 sizeof(struct stf_vin_dev),
 976                 GFP_KERNEL);
 977         if (!stfcamss->vin) {
 978                 ret = -ENOMEM;
 979                 goto err_cam;
 980         }
 981
 982         vin = stfcamss->vin;
 983
 984         vin->irq = platform_get_irq(pdev, 0);
 985         if (vin->irq <= 0) {
 986                 st_err(ST_CAMSS, "Could not get irq\n");
 987                 goto err_cam;
 988         }
 989
 990         vin->isp_irq = platform_get_irq(pdev, 1);
 991         if (vin->isp_irq <= 0) {
 992                 st_err(ST_CAMSS, "Could not get isp irq\n");
 993                 goto err_cam;
 994         }
 995
 996         vin->isp_csi_irq = platform_get_irq(pdev, 2);
 997         if (vin->isp_csi_irq <= 0) {
 998                 st_err(ST_CAMSS, "Could not get isp csi irq\n");
 999                 goto err_cam;
1000         }
1001
1002         vin->isp_scd_irq = platform_get_irq(pdev, 3);
1003         if (vin->isp_scd_irq <= 0) {
1004                 st_err(ST_CAMSS, "Could not get isp scd irq\n");
1005                 goto err_cam;
1006         }
1007
1008         vin->isp_irq_csiline = platform_get_irq(pdev, 4);
1009         if (vin->isp_irq_csiline <= 0) {
1010                 st_err(ST_CAMSS, "Could not get isp irq csiline\n");
1011                 goto err_cam;
1012         }
1013
1014         stfcamss->nclks = ARRAY_SIZE(stfcamss_clocks);
1015         stfcamss->sys_clk = stfcamss_clocks;
1016
1017         ret = devm_clk_bulk_get(dev, stfcamss->nclks, stfcamss->sys_clk);
1018         if (ret) {
1019                 st_err(ST_CAMSS, "Failed to get clk controls\n");
1020                 return ret;
1021         }
1022
1023         stfcamss->nrsts = ARRAY_SIZE(stfcamss_resets);
1024         stfcamss->sys_rst = stfcamss_resets;
1025
1026         ret = devm_reset_control_bulk_get_exclusive(dev, stfcamss->nrsts,
1027                 stfcamss->sys_rst);
1028         if (ret) {
1029                 st_err(ST_CAMSS, "Failed to get reset controls\n");
1030                 return ret;
1031         }
1032
1033         ret = of_parse_phandle_with_fixed_args(dev->of_node,
1034                         "starfive,aon-syscon", 1, 0, &args);
1035         if (ret < 0) {
1036                 st_err(ST_CAMSS, "Failed to parse starfive,aon-syscon\n");
1037                 return -EINVAL;
1038         }
1039
1040         stfcamss->stf_aon_syscon = syscon_node_to_regmap(args.np);
1041         of_node_put(args.np);
1042         if (IS_ERR(stfcamss->stf_aon_syscon))
1043                 return PTR_ERR(stfcamss->stf_aon_syscon);
1044
1045         stfcamss->aon_gp_reg = args.args[0];
1046
1047         ret = stfcamss_get_mem_res(pdev, vin);
1048         if (ret) {
1049                 st_err(ST_CAMSS, "Could not map registers\n");
1050                 goto err_cam;
1051         }
1052
1053         ret = vin_parse_dt(dev, vin);
1054         if (ret)
1055                 goto err_cam;
1056
1057         vin->dev = dev;
1058         stfcamss->dev = dev;
1059         platform_set_drvdata(pdev, stfcamss);
1060
1061         v4l2_async_notifier_init(&stfcamss->notifier);
1062
1063         num_subdevs = stfcamss_of_parse_ports(stfcamss);
1064         if (num_subdevs < 0) {
1065                 ret = num_subdevs;
1066                 goto err_cam_noti;
1067         }
1068
1069         ret = stfcamss_init_subdevices(stfcamss);
1070         if (ret < 0) {
1071                 st_err(ST_CAMSS, "Failed to init subdevice: %d\n", ret);
1072                 goto err_cam_noti;
1073         }
1074
1075         stfcamss->media_dev.dev = stfcamss->dev;
1076         strscpy(stfcamss->media_dev.model, "Starfive Camera Subsystem",
1077                 sizeof(stfcamss->media_dev.model));
1078         strscpy(stfcamss->media_dev.serial, "0123456789ABCDEF",
1079                 sizeof(stfcamss->media_dev.serial));
1080         snprintf(stfcamss->media_dev.bus_info, sizeof(stfcamss->media_dev.bus_info),
1081                         "%s:%s", dev_bus_name(dev), pdev->name);
1082         stfcamss->media_dev.hw_revision = 0x01;
1083         stfcamss->media_dev.ops = &stfcamss_media_ops;
1084         media_device_init(&stfcamss->media_dev);
1085
1086         stfcamss->v4l2_dev.mdev = &stfcamss->media_dev;
1087
1088         ret = v4l2_device_register(stfcamss->dev, &stfcamss->v4l2_dev);
1089         if (ret < 0) {
1090                 st_err(ST_CAMSS, "Failed to register V4L2 device: %d\n", ret);
1091                 goto err_cam_noti_med;
1092         }
1093
1094         ret = stfcamss_register_subdevices(stfcamss);
1095         if (ret < 0) {
1096                 st_err(ST_CAMSS, "Failed to register subdevice: %d\n", ret);
1097                 goto err_cam_noti_med_vreg;
1098         }
1099
1100         if (num_subdevs) {
1101                 stfcamss->notifier.ops = &stfcamss_subdev_notifier_ops;
1102                 ret = v4l2_async_notifier_register(&stfcamss->v4l2_dev,
1103                                 &stfcamss->notifier);
1104                 if (ret) {
1105                         st_err(ST_CAMSS,
1106                                 "Failed to register async subdev nodes: %d\n",
1107                                 ret);
1108                         goto err_cam_noti_med_vreg_sub;
1109                 }
1110         } else {
1111                 ret = v4l2_device_register_subdev_nodes(&stfcamss->v4l2_dev);
1112                 if (ret < 0) {
1113                         st_err(ST_CAMSS,
1114                                 "Failed to register subdev nodes: %d\n",
1115                                 ret);
1116                         goto err_cam_noti_med_vreg_sub;
1117                 }
1118
1119                 ret = media_device_register(&stfcamss->media_dev);
1120                 if (ret < 0) {
1121                         st_err(ST_CAMSS, "Failed to register media device: %d\n",
1122                                         ret);
1123                         goto err_cam_noti_med_vreg_sub_medreg;
1124                 }
1125         }
1126
1127 #ifdef CONFIG_DEBUG_FS
1128         stfcamss->debugfs_entry = debugfs_create_dir("stfcamss", NULL);
1129         stfcamss->vin_debugfs = debugfs_create_file("stf_vin",
1130                         0644, stfcamss->debugfs_entry,
1131                         (void *)dev, &vin_debug_fops);
1132         debugfs_create_u32("dbg_level",
1133                         0644, stfcamss->debugfs_entry,
1134                         &stdbg_level);
1135         debugfs_create_u32("dbg_mask",
1136                         0644, stfcamss->debugfs_entry,
1137                         &stdbg_mask);
1138 #endif
1139         dev_info(dev, "stfcamss probe success!\n");
1140
1141         return 0;
1142
1143 #ifdef CONFIG_DEBUG_FS
1144         debugfs_remove(stfcamss->vin_debugfs);
1145         debugfs_remove_recursive(stfcamss->debugfs_entry);
1146         stfcamss->debugfs_entry = NULL;
1147 #endif
1148
1149 err_cam_noti_med_vreg_sub_medreg:
1150 err_cam_noti_med_vreg_sub:
1151         stfcamss_unregister_subdevices(stfcamss);
1152 err_cam_noti_med_vreg:
1153         v4l2_device_unregister(&stfcamss->v4l2_dev);
1154 err_cam_noti_med:
1155         media_device_cleanup(&stfcamss->media_dev);
1156 err_cam_noti:
1157         v4l2_async_notifier_cleanup(&stfcamss->notifier);
1158 err_cam:
1159         // kfree(stfcamss);
1160         return ret;
1161 }
1162
1163 static int stfcamss_remove(struct platform_device *pdev)
1164 {
1165         struct stfcamss *stfcamss = platform_get_drvdata(pdev);
1166
1167         dev_info(&pdev->dev, "remove done\n");
1168
1169 #ifdef CONFIG_DEBUG_FS
1170         debugfs_remove(stfcamss->vin_debugfs);
1171         debugfs_remove_recursive(stfcamss->debugfs_entry);
1172         stfcamss->debugfs_entry = NULL;
1173 #endif
1174
1175         stfcamss_unregister_subdevices(stfcamss);
1176         v4l2_device_unregister(&stfcamss->v4l2_dev);
1177         media_device_cleanup(&stfcamss->media_dev);
1178
1179         kfree(stfcamss);
1180
1181         return 0;
1182 }
1183
1184 static const struct of_device_id stfcamss_of_match[] = {
1185         { .compatible = "starfive,jh7110-vin" },
1186         { /* end node */ },
1187 };
1188
1189 MODULE_DEVICE_TABLE(of, stfcamss_of_match);
1190
1191 static struct platform_driver stfcamss_driver = {
1192         .probe = stfcamss_probe,
1193         .remove = stfcamss_remove,
1194         .driver = {
1195                 .name = DRV_NAME,
1196                 .of_match_table = of_match_ptr(stfcamss_of_match),
1197         },
1198 };
1199
1200 static int __init stfcamss_init(void)
1201 {
1202         return platform_driver_register(&stfcamss_driver);
1203 }
1204
1205 static void __exit stfcamss_cleanup(void)
1206 {
1207         platform_driver_unregister(&stfcamss_driver);
1208 }
1209
1210 module_init(stfcamss_init);
1211 //fs_initcall(stfcamss_init);
1212 module_exit(stfcamss_cleanup);
1213
1214 MODULE_LICENSE("GPL");