static int stm32_usbphyc_probe()

in st/phy-stm32-usbphyc.c [611:763]

• 123 lines of code
• 20 McCabe index (conditional complexity)

static int stm32_usbphyc_probe(struct platform_device *pdev)
{
	struct stm32_usbphyc *usbphyc;
	struct device *dev = &pdev->dev;
	struct device_node *child, *np = dev->of_node;
	struct phy_provider *phy_provider;
	u32 pllen, version;
	int ret, port = 0;

	usbphyc = devm_kzalloc(dev, sizeof(*usbphyc), GFP_KERNEL);
	if (!usbphyc)
		return -ENOMEM;
	usbphyc->dev = dev;
	dev_set_drvdata(dev, usbphyc);

	usbphyc->base = devm_platform_ioremap_resource(pdev, 0);
	if (IS_ERR(usbphyc->base))
		return PTR_ERR(usbphyc->base);

	usbphyc->clk = devm_clk_get(dev, NULL);
	if (IS_ERR(usbphyc->clk))
		return dev_err_probe(dev, PTR_ERR(usbphyc->clk), "clk get_failed\n");

	ret = clk_prepare_enable(usbphyc->clk);
	if (ret) {
		dev_err(dev, "clk enable failed: %d\n", ret);
		return ret;
	}

	usbphyc->rst = devm_reset_control_get(dev, NULL);
	if (!IS_ERR(usbphyc->rst)) {
		reset_control_assert(usbphyc->rst);
		udelay(2);
		reset_control_deassert(usbphyc->rst);
	} else {
		ret = PTR_ERR(usbphyc->rst);
		if (ret == -EPROBE_DEFER)
			goto clk_disable;

		stm32_usbphyc_clr_bits(usbphyc->base + STM32_USBPHYC_PLL, PLLEN);
	}

	/*
	 * Wait for minimum width of powerdown pulse (ENABLE = Low):
	 * we have to ensure the PLL is disabled before phys initialization.
	 */
	if (readl_relaxed_poll_timeout(usbphyc->base + STM32_USBPHYC_PLL,
				       pllen, !(pllen & PLLEN), 5, 50)) {
		dev_warn(usbphyc->dev, "PLL not reset\n");
		ret = -EPROBE_DEFER;
		goto clk_disable;
	}

	usbphyc->switch_setup = -EINVAL;
	usbphyc->nphys = of_get_child_count(np);
	usbphyc->phys = devm_kcalloc(dev, usbphyc->nphys,
				     sizeof(*usbphyc->phys), GFP_KERNEL);
	if (!usbphyc->phys) {
		ret = -ENOMEM;
		goto clk_disable;
	}

	usbphyc->vdda1v1 = devm_regulator_get(dev, "vdda1v1");
	if (IS_ERR(usbphyc->vdda1v1)) {
		ret = dev_err_probe(dev, PTR_ERR(usbphyc->vdda1v1),
				    "failed to get vdda1v1 supply\n");
		goto clk_disable;
	}

	usbphyc->vdda1v8 = devm_regulator_get(dev, "vdda1v8");
	if (IS_ERR(usbphyc->vdda1v8)) {
		ret = dev_err_probe(dev, PTR_ERR(usbphyc->vdda1v8),
				    "failed to get vdda1v8 supply\n");
		goto clk_disable;
	}

	for_each_child_of_node(np, child) {
		struct stm32_usbphyc_phy *usbphyc_phy;
		struct phy *phy;
		u32 index;

		phy = devm_phy_create(dev, child, &stm32_usbphyc_phy_ops);
		if (IS_ERR(phy)) {
			ret = PTR_ERR(phy);
			if (ret != -EPROBE_DEFER)
				dev_err(dev, "failed to create phy%d: %d\n",
					port, ret);
			goto put_child;
		}

		usbphyc_phy = devm_kzalloc(dev, sizeof(*usbphyc_phy),
					   GFP_KERNEL);
		if (!usbphyc_phy) {
			ret = -ENOMEM;
			goto put_child;
		}

		ret = of_property_read_u32(child, "reg", &index);
		if (ret || index > usbphyc->nphys) {
			dev_err(&phy->dev, "invalid reg property: %d\n", ret);
			goto put_child;
		}

		usbphyc->phys[port] = usbphyc_phy;
		phy_set_bus_width(phy, 8);
		phy_set_drvdata(phy, usbphyc_phy);

		usbphyc->phys[port]->phy = phy;
		usbphyc->phys[port]->usbphyc = usbphyc;
		usbphyc->phys[port]->index = index;
		usbphyc->phys[port]->active = false;

		usbphyc->phys[port]->vbus = devm_regulator_get_optional(&phy->dev, "vbus");
		if (IS_ERR(usbphyc->phys[port]->vbus)) {
			ret = PTR_ERR(usbphyc->phys[port]->vbus);
			if (ret == -EPROBE_DEFER)
				goto put_child;
			usbphyc->phys[port]->vbus = NULL;
		}

		/* Configure phy tuning */
		stm32_usbphyc_phy_tuning(usbphyc, child, index);

		port++;
	}

	phy_provider = devm_of_phy_provider_register(dev,
						     stm32_usbphyc_of_xlate);
	if (IS_ERR(phy_provider)) {
		ret = PTR_ERR(phy_provider);
		dev_err(dev, "failed to register phy provider: %d\n", ret);
		goto clk_disable;
	}

	ret = stm32_usbphyc_clk48_register(usbphyc);
	if (ret) {
		dev_err(dev, "failed to register ck_usbo_48m clock: %d\n", ret);
		goto clk_disable;
	}

	version = readl_relaxed(usbphyc->base + STM32_USBPHYC_VERSION);
	dev_info(dev, "registered rev:%lu.%lu\n",
		 FIELD_GET(MAJREV, version), FIELD_GET(MINREV, version));

	return 0;

put_child:
	of_node_put(child);
clk_disable:
	clk_disable_unprepare(usbphyc->clk);

	return ret;
}