[PATCH RFC v4 7/9] phy: qcom: qmp-pcie: Add link-mode multi-PHY probe infrastructure

[Qiang Yu]

Some QMP PCIe PHY hardware blocks support multiple link topologies (e.g.
x8 or x4+x4) selected via a TCSR register. The existing probe path has
no way to model this: it assumes a single cfg per DT node and instantiates
exactly one PHY.

Introduce a link-mode probe path where match data carries a per-mode cfg
table. At probe time the driver reads the DT-selected mode, looks up the
corresponding cfg array, and instantiates only the sub-PHYs required by
that mode. A #phy-cells = <1> provider is registered so consumers can
address individual sub-PHYs by index.

Three new data structures support this: qmp_pcie_data holds per-provider
state including the phy array, active mode, and regmap for the mode
register; qmp_pcie_link_mode_cfg maps a mode index to its per-PHY cfg
array; qmp_pcie_match_data is the top-level match data for link-mode
platforms.

On the probe side, qmp_pcie_probe() is reworked to instantiate one
qmp_pcie per active sub-PHY and register the appropriate clock and phy
providers. Per-instance DT parsing and phy object creation are factored
into helpers to keep the probe path clean. The active link mode is written
to the TCSR register at power-on to handle re-initialisation after
low-power transitions.

Platforms without a "link-mode" property continue to use the existing
single-cfg path and of_phy_simple_xlate unchanged.

Signed-off-by: Qiang Yu <qiang.yu@xxxxxxxxxxxxxxxx>
---
 drivers/phy/qualcomm/phy-qcom-qmp-pcie.c | 351 +++++++++++++++++++++++++++----
 1 file changed, 311 insertions(+), 40 deletions(-)

diff --git a/drivers/phy/qualcomm/phy-qcom-qmp-pcie.c b/drivers/phy/qualcomm/phy-qcom-qmp-pcie.c
index b100302be12a..d78d57fb64d6 100644
--- a/drivers/phy/qualcomm/phy-qcom-qmp-pcie.c
+++ b/drivers/phy/qualcomm/phy-qcom-qmp-pcie.c
@@ -12,6 +12,7 @@
 #include <linux/kernel.h>
 #include <linux/mfd/syscon.h>
 #include <linux/module.h>
+#include <linux/mutex.h>
 #include <linux/of.h>
 #include <linux/of_address.h>
 #include <linux/phy/pcie.h>
@@ -3342,6 +3343,28 @@ struct qmp_pcie {
 	struct clk_fixed_rate aux_clk_fixed;
 };
 
+struct qmp_pcie_data {
+	struct phy **phys;
+	u32 active_link_mode;
+	int num_phys;
+	struct regmap *link_mode_map;
+	u32 link_mode_offset;
+	struct mutex link_mode_lock;
+
+	int num_pipe_outputs;
+	struct clk_fixed_rate *pipe_out_clks;
+};
+
+struct qmp_pcie_link_mode_cfg {
+	const struct qmp_phy_cfg *cfgs[QMP_PHY_SELECTOR_1 + 1];
+	u32 num_phys;
+};
+
+struct qmp_pcie_match_data {
+	const struct qmp_pcie_link_mode_cfg *mode_cfgs;
+	u32 num_modes;
+};
+
 static bool qphy_checkbits(const void __iomem *base, u32 offset, u32 val)
 {
 	u32 reg;
@@ -4897,6 +4920,27 @@ static int qmp_pcie_exit(struct phy *phy)
 	return 0;
 }
 
+static int qmp_pcie_config_link_mode(struct qmp_pcie *qmp)
+{
+	struct qmp_pcie_data *qmp_data = dev_get_drvdata(qmp->dev);
+	int ret;
+
+	if (!qmp_data)
+		return 0;
+
+	mutex_lock(&qmp_data->link_mode_lock);
+
+	ret = regmap_write(qmp_data->link_mode_map, qmp_data->link_mode_offset,
+			   qmp_data->active_link_mode);
+	if (ret)
+		goto out_unlock;
+
+out_unlock:
+	mutex_unlock(&qmp_data->link_mode_lock);
+
+	return ret;
+}
+
 static int qmp_pcie_power_on(struct phy *phy)
 {
 	struct qmp_pcie *qmp = phy_get_drvdata(phy);
@@ -4907,6 +4951,10 @@ static int qmp_pcie_power_on(struct phy *phy)
 	unsigned int mask, val;
 	int ret;
 
+	ret = qmp_pcie_config_link_mode(qmp);
+	if (ret)
+		return ret;
+
 	/*
 	 * Write CSR register for PHY that doesn't support no_csr reset or has not
 	 * been initialized.
@@ -5229,6 +5277,20 @@ static struct clk_hw *qmp_pcie_clk_hw_get(struct of_phandle_args *clkspec, void
 	return ERR_PTR(-EINVAL);
 }
 
+static struct clk_hw *qmp_pcie_clk_hw_get_link_mode(struct of_phandle_args *clkspec, void *data)
+{
+	struct qmp_pcie_data *qmp_data = data;
+	unsigned int idx = 0;
+
+	if (clkspec->args_count)
+		idx = clkspec->args[0];
+
+	if (idx < (unsigned int)qmp_data->num_pipe_outputs)
+		return &qmp_data->pipe_out_clks[idx].hw;
+
+	return ERR_PTR(-EINVAL);
+}
+
 static int qmp_pcie_register_clocks(struct qmp_pcie *qmp, struct device_node *np)
 {
 	int ret;
@@ -5258,6 +5320,37 @@ static int qmp_pcie_register_clocks(struct qmp_pcie *qmp, struct device_node *np
 	return devm_add_action_or_reset(qmp->dev, phy_clk_release_provider, np);
 }
 
+static int qmp_pcie_register_clocks_link_mode(struct device *dev,
+					      struct device_node *np,
+					      struct qmp_pcie_data *qmp_data)
+{
+	int num_pipe_outputs;
+	int i;
+	int ret;
+
+	num_pipe_outputs = of_property_count_strings(np, "clock-output-names");
+	if (num_pipe_outputs < 0)
+		num_pipe_outputs = 1;
+
+	qmp_data->num_pipe_outputs = num_pipe_outputs;
+	qmp_data->pipe_out_clks = devm_kcalloc(dev, num_pipe_outputs,
+					       sizeof(*qmp_data->pipe_out_clks), GFP_KERNEL);
+	if (!qmp_data->pipe_out_clks)
+		return -ENOMEM;
+
+	for (i = 0; i < num_pipe_outputs; i++) {
+		ret = __phy_pipe_clk_register(dev, np, i, &qmp_data->pipe_out_clks[i]);
+		if (ret)
+			return ret;
+	}
+
+	ret = of_clk_add_hw_provider(np, qmp_pcie_clk_hw_get_link_mode, qmp_data);
+	if (ret)
+		return ret;
+
+	return devm_add_action_or_reset(dev, phy_clk_release_provider, np);
+}
+
 static int qmp_pcie_parse_dt_legacy(struct qmp_pcie *qmp, struct device_node *np)
 {
 	struct platform_device *pdev = to_platform_device(qmp->dev);
@@ -5437,36 +5530,102 @@ static int qmp_pcie_parse_dt(struct qmp_pcie *qmp)
 	return 0;
 }
 
-static int qmp_pcie_probe(struct platform_device *pdev)
+static int qmp_pcie_read_link_mode(struct device *dev, struct regmap **mode_map,
+				   u32 *mode_offset,
+				   u32 *active_link_mode,
+				   u32 *hw_link_mode)
 {
-	struct dev_pm_domain_list *pd_list;
-	struct device *dev = &pdev->dev;
-	struct phy_provider *phy_provider;
-	struct device_node *np;
-	struct qmp_pcie *qmp;
-	struct phy *phy;
+	struct regmap *map;
+	unsigned int args[2];
+	unsigned int mode;
 	int ret;
 
-	qmp = devm_kzalloc(dev, sizeof(*qmp), GFP_KERNEL);
-	if (!qmp)
+	map = syscon_regmap_lookup_by_phandle_args(dev->of_node, "qcom,link-mode",
+						   ARRAY_SIZE(args), args);
+	if (IS_ERR(map))
+		return PTR_ERR(map);
+
+	ret = regmap_read(map, args[0], &mode);
+	if (ret)
+		return ret;
+
+	*mode_map = map;
+	*mode_offset = args[0];
+	*active_link_mode = args[1];
+	*hw_link_mode = mode;
+
+	return 0;
+}
+
+static int qmp_pcie_validate_link_mode(struct device *dev,
+				       const struct qmp_pcie_link_mode_cfg *mode_cfg,
+				       u32 active_link_mode, u32 hw_link_mode)
+{
+	int i;
+
+	if (active_link_mode == hw_link_mode)
+		return 0;
+
+	for (i = 0; i < mode_cfg->num_phys; i++) {
+		const struct qmp_phy_cfg *cfg = mode_cfg->cfgs[i];
+
+		if (!cfg || !cfg->tbls.serdes_num) {
+			dev_err(dev,
+				"missing phy settings for link-mode %u, logical-phy %d (hw=%u)\n",
+				active_link_mode, i, hw_link_mode);
+			return -EINVAL;
+		}
+	}
+
+	return 0;
+}
+
+static int qmp_pcie_parse_dt_non_primary(struct qmp_pcie *qmp)
+{
+	struct platform_device *pdev = to_platform_device(qmp->dev);
+	struct device *dev = qmp->dev;
+	char *pipe_clk_name;
+	char *pipediv2_clk_name;
+	void __iomem *base;
+
+	base = devm_platform_ioremap_resource(pdev, qmp->id);
+	if (IS_ERR(base))
+		return PTR_ERR(base);
+
+	pipe_clk_name = devm_kasprintf(dev, GFP_KERNEL, "pipe_%c", 'a' + qmp->id);
+	pipediv2_clk_name = devm_kasprintf(dev, GFP_KERNEL, "pipediv2_%c", 'a' + qmp->id);
+	if (!pipe_clk_name || !pipediv2_clk_name)
 		return -ENOMEM;
 
-	qmp->dev = dev;
+	return qmp_pcie_parse_dt_common(qmp, base, pipe_clk_name, pipediv2_clk_name);
+}
+
+static int qmp_pcie_probe_phy(struct qmp_pcie *qmp, struct device_node *np,
+			      struct phy **out_phy)
+{
+	struct device *dev = qmp->dev;
+	struct device_node *phy_np;
+	const struct qmp_phy_cfg *cfg = qmp->cfg;
+	int ret;
 
-	qmp->cfg = of_device_get_match_data(dev);
-	if (!qmp->cfg)
+	if (!cfg)
 		return -EINVAL;
 
-	WARN_ON_ONCE(!qmp->cfg->pwrdn_ctrl);
-	WARN_ON_ONCE(!qmp->cfg->phy_status);
+	WARN_ON_ONCE(!cfg->pwrdn_ctrl);
+	WARN_ON_ONCE(!cfg->phy_status);
 
-	ret = devm_pm_domain_attach_list(dev, NULL, &pd_list);
-	if (ret < 0 && ret != -EEXIST) {
-		dev_err(dev, "Failed to attach power domain\n");
-		return ret;
-	}
+	qmp->mode = PHY_MODE_PCIE_RC;
 
-	ret = devm_pm_runtime_enable(dev);
+	if (qmp->id == QMP_PHY_SELECTOR_0) {
+		phy_np = np;
+		if (np != dev->of_node)
+			ret = qmp_pcie_parse_dt_legacy(qmp, np);
+		else
+			ret = qmp_pcie_parse_dt(qmp);
+	} else {
+		phy_np = dev->of_node;
+		ret = qmp_pcie_parse_dt_non_primary(qmp);
+	}
 	if (ret)
 		return ret;
 
@@ -5482,35 +5641,147 @@ static int qmp_pcie_probe(struct platform_device *pdev)
 	if (ret)
 		return ret;
 
-	/* Check for legacy binding with child node. */
-	np = of_get_next_available_child(dev->of_node, NULL);
-	if (np) {
-		ret = qmp_pcie_parse_dt_legacy(qmp, np);
-	} else {
-		np = of_node_get(dev->of_node);
-		ret = qmp_pcie_parse_dt(qmp);
+	*out_phy = devm_phy_create(dev, phy_np, &qmp_pcie_phy_ops);
+	if (IS_ERR(*out_phy)) {
+		ret = PTR_ERR(*out_phy);
+		return ret;
 	}
-	if (ret)
-		goto err_node_put;
 
-	ret = qmp_pcie_register_clocks(qmp, np);
+	phy_set_drvdata(*out_phy, qmp);
+
+	return 0;
+}
+
+static struct phy *qmp_pcie_link_mode_xlate(struct device *dev,
+					    const struct of_phandle_args *args)
+{
+	struct qmp_pcie_data *qmp_data = dev_get_drvdata(dev);
+	unsigned int idx;
+
+	if (!qmp_data)
+		return ERR_PTR(-EINVAL);
+
+	if (args->args_count < 1)
+		return ERR_PTR(-EINVAL);
+
+	idx = args->args[0];
+
+	if (idx < (unsigned int)qmp_data->num_phys)
+		return qmp_data->phys[idx] ?: ERR_PTR(-EINVAL);
+
+	return ERR_PTR(-EINVAL);
+}
+
+static int qmp_pcie_probe(struct platform_device *pdev)
+{
+	struct dev_pm_domain_list *pd_list;
+	struct device *dev = &pdev->dev;
+	struct device_node *np = NULL;
+	struct phy_provider *phy_provider;
+	const struct qmp_phy_cfg *cfg = NULL;
+	const struct qmp_pcie_match_data *mode_data;
+	const struct qmp_pcie_link_mode_cfg *mode_cfg;
+	const void *match_data;
+	struct qmp_pcie_data *qmp_data = NULL;
+	struct regmap *link_mode_map = NULL;
+	struct qmp_pcie *qmp;
+	struct phy **phys;
+	u32 link_mode_offset = 0;
+	u32 hw_link_mode = 0;
+	u32 link_mode = 0;
+	bool use_link_mode = false;
+	int i;
+	int num_phys = 1;
+	int ret;
+
+	ret = devm_pm_domain_attach_list(dev, NULL, &pd_list);
+	if (ret < 0 && ret != -EEXIST) {
+		dev_err(dev, "Failed to attach power domain\n");
+		return ret;
+	}
+
+	ret = devm_pm_runtime_enable(dev);
 	if (ret)
-		goto err_node_put;
+		return ret;
 
-	qmp->mode = PHY_MODE_PCIE_RC;
+	match_data = of_device_get_match_data(dev);
+	if (!match_data)
+		return -EINVAL;
+
+	ret = qmp_pcie_read_link_mode(dev, &link_mode_map, &link_mode_offset,
+				      &link_mode, &hw_link_mode);
+	if (ret == -ENOENT)
+		cfg = match_data;
+	else if (ret)
+		return dev_err_probe(dev, ret, "failed to read qcom,link-mode\n");
+
+	if (!ret) {
+		use_link_mode = true;
+		mode_data = match_data;
+		if (link_mode >= mode_data->num_modes) {
+			dev_err(dev, "invalid qcom,link-mode: %u\n", link_mode);
+			return -EINVAL;
+		}
 
-	phy = devm_phy_create(dev, np, &qmp_pcie_phy_ops);
-	if (IS_ERR(phy)) {
-		ret = PTR_ERR(phy);
-		dev_err(dev, "failed to create PHY: %d\n", ret);
-		goto err_node_put;
+		mode_cfg = &mode_data->mode_cfgs[link_mode];
+		num_phys = mode_cfg->num_phys;
+
+		ret = qmp_pcie_validate_link_mode(dev, mode_cfg, link_mode, hw_link_mode);
+		if (ret)
+			return ret;
 	}
 
-	phy_set_drvdata(phy, qmp);
+	qmp = devm_kcalloc(dev, num_phys, sizeof(*qmp), GFP_KERNEL);
+	if (!qmp)
+		return -ENOMEM;
 
-	of_node_put(np);
+	phys = devm_kcalloc(dev, num_phys, sizeof(*phys), GFP_KERNEL);
+	if (!phys)
+		return -ENOMEM;
+
+	if (use_link_mode) {
+		qmp_data = devm_kzalloc(dev, sizeof(*qmp_data), GFP_KERNEL);
+		if (!qmp_data)
+			return -ENOMEM;
+		qmp_data->phys = phys;
+		qmp_data->active_link_mode = link_mode;
+		qmp_data->link_mode_map = link_mode_map;
+		qmp_data->link_mode_offset = link_mode_offset;
+		qmp_data->num_phys = num_phys;
+		mutex_init(&qmp_data->link_mode_lock);
+	}
+
+	np = of_get_next_available_child(dev->of_node, NULL);
+	if (!np)
+		np = of_node_get(dev->of_node);
+
+	for (i = 0; i < num_phys; i++) {
+		const struct qmp_phy_cfg *phy_cfg = use_link_mode ? mode_cfg->cfgs[i] : cfg;
 
-	phy_provider = devm_of_phy_provider_register(dev, of_phy_simple_xlate);
+		qmp[i].dev = dev;
+		qmp[i].id = i;
+		qmp[i].cfg = phy_cfg;
+		ret = qmp_pcie_probe_phy(&qmp[i], np, &phys[i]);
+		if (ret)
+			goto err_node_put;
+	}
+
+	if (use_link_mode) {
+		ret = qmp_pcie_register_clocks_link_mode(dev, np, qmp_data);
+		if (ret)
+			goto err_node_put;
+
+		dev_set_drvdata(dev, qmp_data);
+		phy_provider = devm_of_phy_provider_register(dev, qmp_pcie_link_mode_xlate);
+	} else {
+		ret = qmp_pcie_register_clocks(qmp, np);
+		if (ret)
+			goto err_node_put;
+
+		phy_provider = devm_of_phy_provider_register(dev, of_phy_simple_xlate);
+	}
+
+	of_node_put(np);
 
 	return PTR_ERR_OR_ZERO(phy_provider);
 

-- 
2.34.1