Add CSIPHY driver routines to catch any possible csiphy subdevices
under CAMSS device tree node.
Signed-off-by: Vladimir Zapolskiy <vladimir.zapolskiy@xxxxxxxxxx>
---
.../media/platform/qcom/camss/camss-csiphy.c | 248 ++++++++++++++++++
.../media/platform/qcom/camss/camss-csiphy.h | 3 +
drivers/media/platform/qcom/camss/camss.c | 2 +
3 files changed, 253 insertions(+)
diff --git a/drivers/media/platform/qcom/camss/camss-csiphy.c b/drivers/media/platform/qcom/camss/camss-csiphy.c
index f561811b7617..3020f7d0f621 100644
--- a/drivers/media/platform/qcom/camss/camss-csiphy.c
+++ b/drivers/media/platform/qcom/camss/camss-csiphy.c
@@ -17,6 +17,7 @@
#include <linux/pm_runtime.h>
#include <media/media-entity.h>
#include <media/v4l2-device.h>
+#include <media/v4l2-fwnode.h>
#include <media/v4l2-subdev.h>
#include "camss-csiphy.h"
@@ -24,6 +25,17 @@
#define MSM_CSIPHY_NAME "msm_csiphy"
+struct csiphy_priv {
+ struct device *dev;
+ unsigned int id;
+ struct camss *camss;
+ struct csiphy_device *csiphy;
+
+ struct v4l2_async_notifier notifier;
+
+ bool combo_mode;
+};
+
static const struct csiphy_format_info formats_8x16[] = {
{ MEDIA_BUS_FMT_UYVY8_1X16, 8 },
{ MEDIA_BUS_FMT_VYUY8_1X16, 8 },
@@ -836,3 +848,239 @@ void msm_csiphy_unregister_entity(struct csiphy_device *csiphy)
v4l2_device_unregister_subdev(&csiphy->subdev);
media_entity_cleanup(&csiphy->subdev.entity);
}
+
+static int csiphy_notify_bound(struct v4l2_async_notifier *notifier,
+ struct v4l2_subdev *subdev,
+ struct v4l2_async_connection *asd)
+{
+ struct csiphy_priv *csiphy_priv = container_of(notifier,
+ struct csiphy_priv, notifier);
+ struct camss_async_subdev *csd = container_of(asd,
+ struct camss_async_subdev, asd);
+ struct csiphy_device *csiphy = csiphy_priv->csiphy;
+ struct media_entity *sensor = &subdev->entity;
+ unsigned int i;
+
+ /* Keep parsed media interface data, but set the correct port id */
+ csiphy->id = csiphy_priv->id;
+ csiphy->cfg.csi2 = &csd->interface.csi2;
+
+ for (i = 0; i < sensor->num_pads; i++)
+ if (sensor->pads[i].flags & MEDIA_PAD_FL_SOURCE)
+ break;
+
+ if (i == sensor->num_pads) {
+ dev_err(csiphy_priv->dev, "No source pad in external entity\n");
+ return -EINVAL;
+ }
+
+ return media_create_pad_link(sensor, i, &csiphy->subdev.entity,
+ MSM_CSIPHY_PAD_SINK,
+ MEDIA_LNK_FL_IMMUTABLE | MEDIA_LNK_FL_ENABLED);
+}
+
+static int csiphy_notify_complete(struct v4l2_async_notifier *notifier)
+{
+ struct csiphy_priv *csiphy = container_of(notifier, struct csiphy_priv,
+ notifier);
+ struct camss *camss = csiphy->camss;
+
+ return v4l2_device_register_subdev_nodes(&camss->v4l2_dev);
+}
+
+static const struct v4l2_async_notifier_operations csiphy_notify_ops = {
+ .bound = csiphy_notify_bound,
+ .complete = csiphy_notify_complete,
+};
+
+static int msm_csiphy_parse_ports(struct csiphy_priv *csiphy)
+{
+ struct device *dev = csiphy->dev;
+ struct fwnode_handle *fwnode = dev_fwnode(dev), *ep;
+ unsigned int num_endpoints = fwnode_graph_get_endpoint_count(fwnode,
+ FWNODE_GRAPH_DEVICE_DISABLED);
+ int ret;
+
+ switch (num_endpoints) {
+ case 0:
+ return 0;
+ case 1:
+ break;
+ case 2:
+ csiphy->combo_mode = true;
+ break;
+ default:
+ return -EINVAL;
+ }
+
+ v4l2_async_nf_init(&csiphy->notifier, &csiphy->camss->v4l2_dev);
+ csiphy->notifier.ops = &csiphy_notify_ops;
+
+ fwnode_graph_for_each_endpoint(fwnode, ep) {
+ struct camss_async_subdev *csd;
+
+ csd = v4l2_async_nf_add_fwnode_remote(&csiphy->notifier, ep,
+ struct camss_async_subdev);
+ if (IS_ERR(csd)) {
+ ret = PTR_ERR(csd);
+ goto err_remote;
+ }
+
+ ret = camss_parse_endpoint_node(dev, ep, csd);
+ if (ret < 0)
+ goto err_remote;
+ }
+
+ ret = v4l2_async_nf_register(&csiphy->notifier);
+ if (ret)
+ goto err_cleanup;
+
+ return 0;
+
+err_remote:
+ fwnode_handle_put(ep);
+err_cleanup:
+ v4l2_async_nf_cleanup(&csiphy->notifier);
+
+ return ret;
+}
+
+static int msm_csiphy_init(struct csiphy_priv *csiphy)
+{
+ struct camss *camss = csiphy->camss;
+ unsigned int i = csiphy->id, j;
+ int ret;
+
+ ret = msm_csiphy_subdev_init(camss, &camss->csiphy[i], i);
+ if (ret < 0) {
+ dev_err(csiphy->dev, "Failed to init csiphy%d sub-device: %d\n",
+ i, ret);
+ return ret;
+ }
+
+ ret = msm_csiphy_register_entity(&camss->csiphy[i], &camss->v4l2_dev);
+ if (ret < 0) {
+ dev_err(csiphy->dev, "Failed to register csiphy%d entity: %d\n",
+ i, ret);
+ return ret;
+ }
+
+ for (j = 0; j < camss->res->csid_num; j++) {
+ ret = media_create_pad_link(&camss->csiphy[i].subdev.entity,
+ 1 /* source */,
+ &camss->csid[j].subdev.entity,
+ 0 /* sink */,
+ 0);
+ if (ret < 0) {
+ dev_err(csiphy->dev,
+ "Failed to link csiphy%d to csid: %d\n",
+ i, ret);
+ msm_csiphy_unregister_entity(&camss->csiphy[i]);
+ return ret;
+ }
+ }
+
+ return 0;
+}
+
+static int msm_csiphy_probe(struct platform_device *pdev)
+{
+ struct device *dev = &pdev->dev;
+ struct device_node *phy_node = dev_of_node(dev), *camss_node;
+ struct csiphy_priv *csiphy_priv;
+ struct of_phandle_iterator it;
+ struct camss *camss;
+ unsigned int i = 0;
+ int ret;
+
+ /* Bail out if camss device driver has not yet been registered */
+ camss = dev_get_drvdata(dev->parent);
+ if (!camss || !camss->v4l2_dev.dev)
+ return -EPROBE_DEFER;
+
+ camss_node = of_get_parent(phy_node);
+ if (!camss_node)
+ return -ENODEV;
+
+ of_for_each_phandle(&it, ret, camss_node, "phys", "#phy-cells", -1) {
+ if (it.node != phy_node) {
+ i++;
+ continue;
+ }
+
+ if (!of_node_name_eq(it.node, "phy") ||
+ !of_device_is_available(it.node))
+ ret = -ENODEV;
+
+ of_node_put(it.node);
+ break;
+ }
+ of_node_put(camss_node);
+
+ if (ret)
+ return ret;
+
+ if (i >= camss->res->csiphy_num)
+ return -EINVAL;
+
+ csiphy_priv = devm_kzalloc(dev, sizeof(*csiphy_priv), GFP_KERNEL);
+ if (!csiphy_priv)
+ return -ENOMEM;
+
+ csiphy_priv->dev = dev;
+ csiphy_priv->camss = camss;
+ csiphy_priv->id = i;
+ csiphy_priv->csiphy = &camss->csiphy[i];
+
+ ret = msm_csiphy_init(csiphy_priv);
+ if (ret < 0)
+ goto err_parse;
+
+ ret = msm_csiphy_parse_ports(csiphy_priv);
+ if (ret < 0)
+ goto err_cleanup;
+
+ dev_set_drvdata(dev, csiphy_priv);
+
+ return 0;
+
+err_cleanup:
+ msm_csiphy_unregister_entity(csiphy_priv->csiphy);
+err_parse:
+ v4l2_async_nf_unregister(&csiphy_priv->notifier);
+ v4l2_async_nf_cleanup(&csiphy_priv->notifier);
+
+ return ret;
+}
+
+static void msm_csiphy_remove(struct platform_device *pdev)
+{
+ struct csiphy_priv *csiphy_priv = dev_get_drvdata(&pdev->dev);
+
+ msm_csiphy_unregister_entity(csiphy_priv->csiphy);
+
+ v4l2_async_nf_unregister(&csiphy_priv->notifier);
+ v4l2_async_nf_cleanup(&csiphy_priv->notifier);
+}
+
+static const struct of_device_id csiphy_dt_match[] = {
+ { .compatible = "qcom,csiphy", },
+ {}
+};
+
+static struct platform_driver csiphy_platform_driver = {
+ .probe = msm_csiphy_probe,
+ .remove = msm_csiphy_remove,
+ .driver = {
+ .name = "qcom-csiphy",
+ .of_match_table = csiphy_dt_match,
+ },
+};
+
+void __init msm_csiphy_driver_register(void) {
+ platform_driver_register(&csiphy_platform_driver);
+}
+
+void __exit msm_csiphy_driver_unregister(void) {
+ platform_driver_unregister(&csiphy_platform_driver);
+}
diff --git a/drivers/media/platform/qcom/camss/camss-csiphy.h b/drivers/media/platform/qcom/camss/camss-csiphy.h
index f092b7ff2f26..b984aa745c78 100644
--- a/drivers/media/platform/qcom/camss/camss-csiphy.h
+++ b/drivers/media/platform/qcom/camss/camss-csiphy.h
@@ -120,6 +120,9 @@ int msm_csiphy_register_entity(struct csiphy_device *csiphy,
void msm_csiphy_unregister_entity(struct csiphy_device *csiphy);
+void msm_csiphy_driver_register(void);
+void msm_csiphy_driver_unregister(void);
+
extern const struct csiphy_formats csiphy_formats_8x16;
extern const struct csiphy_formats csiphy_formats_8x96;
extern const struct csiphy_formats csiphy_formats_sdm845;
diff --git a/drivers/media/platform/qcom/camss/camss.c b/drivers/media/platform/qcom/camss/camss.c
index 40bb20bbe8b4..57a522fcb8c0 100644
--- a/drivers/media/platform/qcom/camss/camss.c
+++ b/drivers/media/platform/qcom/camss/camss.c
@@ -3955,6 +3955,7 @@ static struct platform_driver qcom_camss_driver = {
static int __init qcom_camss_init(void)
{
+ msm_csiphy_driver_register();
return platform_driver_register(&qcom_camss_driver);
}
module_init(qcom_camss_init);
@@ -3962,6 +3963,7 @@ module_init(qcom_camss_init);
static void __exit qcom_camss_exit(void)
{
platform_driver_unregister(&qcom_camss_driver);
+ msm_csiphy_driver_unregister();
}
module_exit(qcom_camss_exit);
--
2.49.0
