[PATCH v2] phy: zynqmp: Handle the clock enable/disable properly

[Manish Narani]

Gentle Ping!

> -----Original Message-----
> From: Manish Narani <manish.narani at xilinx.com>
> Sent: Tuesday, March 9, 2021 4:48 PM
> To: laurent.pinchart at ideasonboard.com; kishon at ti.com; vkoul at kernel.org;
> Michal Simek <michals at xilinx.com>
> Cc: linux-kernel at vger.kernel.org; linux-arm-kernel at lists.infradead.org; git
> <git at xilinx.com>; Manish Narani <MNARANI at xilinx.com>
> Subject: [PATCH v2] phy: zynqmp: Handle the clock enable/disable properly
> 
> The current driver is not handling the clock enable/disable operations
> properly. The clocks need to be handled correctly by enabling or
> disabling at appropriate places. This patch adds code to handle the
> same.
> 
> Signed-off-by: Manish Narani <manish.narani at xilinx.com>
> ---
>  drivers/phy/xilinx/phy-zynqmp.c | 57
> ++++++++++++++++++++++++++++++++++++-----
>  1 file changed, 50 insertions(+), 7 deletions(-)
> 
> diff --git a/drivers/phy/xilinx/phy-zynqmp.c b/drivers/phy/xilinx/phy-
> zynqmp.c
> index 2b65f84..37fcecf 100644
> --- a/drivers/phy/xilinx/phy-zynqmp.c
> +++ b/drivers/phy/xilinx/phy-zynqmp.c
> @@ -219,6 +219,7 @@ struct xpsgtr_dev {
>  	struct mutex gtr_mutex; /* mutex for locking */
>  	struct xpsgtr_phy phys[NUM_LANES];
>  	const struct xpsgtr_ssc *refclk_sscs[NUM_LANES];
> +	struct clk *clk[NUM_LANES];
>  	bool tx_term_fix;
>  	unsigned int saved_icm_cfg0;
>  	unsigned int saved_icm_cfg1;
> @@ -818,11 +819,15 @@ static struct phy *xpsgtr_xlate(struct device *dev,
>  static int __maybe_unused xpsgtr_suspend(struct device *dev)
>  {
>  	struct xpsgtr_dev *gtr_dev = dev_get_drvdata(dev);
> +	unsigned int i;
> 
>  	/* Save the snapshot ICM_CFG registers. */
>  	gtr_dev->saved_icm_cfg0 = xpsgtr_read(gtr_dev, ICM_CFG0);
>  	gtr_dev->saved_icm_cfg1 = xpsgtr_read(gtr_dev, ICM_CFG1);
> 
> +	for (i = 0; i < ARRAY_SIZE(gtr_dev->clk); i++)
> +		clk_disable_unprepare(gtr_dev->clk[i]);
> +
>  	return 0;
>  }
> 
> @@ -832,6 +837,13 @@ static int __maybe_unused xpsgtr_resume(struct
> device *dev)
>  	unsigned int icm_cfg0, icm_cfg1;
>  	unsigned int i;
>  	bool skip_phy_init;
> +	int err;
> +
> +	for (i = 0; i < ARRAY_SIZE(gtr_dev->clk); i++) {
> +		err = clk_prepare_enable(gtr_dev->clk[i]);
> +		if (err)
> +			goto err_clk_put;
> +	}
> 
>  	icm_cfg0 = xpsgtr_read(gtr_dev, ICM_CFG0);
>  	icm_cfg1 = xpsgtr_read(gtr_dev, ICM_CFG1);
> @@ -852,6 +864,12 @@ static int __maybe_unused xpsgtr_resume(struct
> device *dev)
>  		gtr_dev->phys[i].skip_phy_init = skip_phy_init;
> 
>  	return 0;
> +
> +err_clk_put:
> +	for (i = 0; i < ARRAY_SIZE(gtr_dev->clk); i++)
> +		clk_disable_unprepare(gtr_dev->clk[i]);
> +
> +	return err;
>  }
> 
>  static const struct dev_pm_ops xpsgtr_pm_ops = {
> @@ -865,6 +883,7 @@ static const struct dev_pm_ops xpsgtr_pm_ops = {
>  static int xpsgtr_get_ref_clocks(struct xpsgtr_dev *gtr_dev)
>  {
>  	unsigned int refclk;
> +	int ret;
> 
>  	for (refclk = 0; refclk < ARRAY_SIZE(gtr_dev->refclk_sscs); ++refclk) {
>  		unsigned long rate;
> @@ -874,14 +893,22 @@ static int xpsgtr_get_ref_clocks(struct xpsgtr_dev
> *gtr_dev)
> 
>  		snprintf(name, sizeof(name), "ref%u", refclk);
>  		clk = devm_clk_get_optional(gtr_dev->dev, name);
> -		if (IS_ERR(clk))
> -			return dev_err_probe(gtr_dev->dev, PTR_ERR(clk),
> -					     "Failed to get reference clock
> %u\n",
> -					     refclk);
> +		if (IS_ERR(clk)) {
> +			ret = dev_err_probe(gtr_dev->dev, PTR_ERR(clk),
> +					    "Failed to get reference clock
> %u\n",
> +					    refclk);
> +			goto err_clk_put;
> +		}
> 
>  		if (!clk)
>  			continue;
> 
> +		gtr_dev->clk[refclk] = clk;
> +
> +		ret = clk_prepare_enable(gtr_dev->clk[refclk]);
> +		if (ret)
> +			goto err_clk_put;
> +
>  		/*
>  		 * Get the spread spectrum (SSC) settings for the reference
>  		 * clock rate.
> @@ -899,11 +926,18 @@ static int xpsgtr_get_ref_clocks(struct xpsgtr_dev
> *gtr_dev)
>  			dev_err(gtr_dev->dev,
>  				"Invalid rate %lu for reference clock %u\n",
>  				rate, refclk);
> -			return -EINVAL;
> +			ret = -EINVAL;
> +			goto err_clk_put;
>  		}
>  	}
> 
>  	return 0;
> +
> +err_clk_put:
> +	for (refclk = 0; refclk < ARRAY_SIZE(gtr_dev->clk); refclk++)
> +		clk_disable_unprepare(gtr_dev->clk[refclk]);
> +
> +	return ret;
>  }
> 
>  static int xpsgtr_probe(struct platform_device *pdev)
> @@ -912,6 +946,7 @@ static int xpsgtr_probe(struct platform_device *pdev)
>  	struct xpsgtr_dev *gtr_dev;
>  	struct phy_provider *provider;
>  	unsigned int port;
> +	unsigned int i;
>  	int ret;
> 
>  	gtr_dev = devm_kzalloc(&pdev->dev, sizeof(*gtr_dev), GFP_KERNEL);
> @@ -951,7 +986,8 @@ static int xpsgtr_probe(struct platform_device *pdev)
>  		phy = devm_phy_create(&pdev->dev, np, &xpsgtr_phyops);
>  		if (IS_ERR(phy)) {
>  			dev_err(&pdev->dev, "failed to create PHY\n");
> -			return PTR_ERR(phy);
> +			ret = PTR_ERR(phy);
> +			goto err_clk_put;
>  		}
> 
>  		gtr_phy->phy = phy;
> @@ -962,9 +998,16 @@ static int xpsgtr_probe(struct platform_device
> *pdev)
>  	provider = devm_of_phy_provider_register(&pdev->dev,
> xpsgtr_xlate);
>  	if (IS_ERR(provider)) {
>  		dev_err(&pdev->dev, "registering provider failed\n");
> -		return PTR_ERR(provider);
> +		ret = PTR_ERR(provider);
> +		goto err_clk_put;
>  	}
>  	return 0;
> +
> +err_clk_put:
> +	for (i = 0; i < ARRAY_SIZE(gtr_dev->clk); i++)
> +		clk_disable_unprepare(gtr_dev->clk[i]);
> +
> +	return ret;
>  }
> 
>  static const struct of_device_id xpsgtr_of_match[] = {
> --
> 2.1.1