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