static int pwrap_probe()

in mediatek/mtk-pmic-wrap.c [2112:2274]

• 122 lines of code
• 26 McCabe index (conditional complexity)

static int pwrap_probe(struct platform_device *pdev)
{
	int ret, irq;
	u32 mask_done;
	struct pmic_wrapper *wrp;
	struct device_node *np = pdev->dev.of_node;
	const struct of_device_id *of_slave_id = NULL;
	struct resource *res;

	if (np->child)
		of_slave_id = of_match_node(of_slave_match_tbl, np->child);

	if (!of_slave_id) {
		dev_dbg(&pdev->dev, "slave pmic should be defined in dts\n");
		return -EINVAL;
	}

	wrp = devm_kzalloc(&pdev->dev, sizeof(*wrp), GFP_KERNEL);
	if (!wrp)
		return -ENOMEM;

	platform_set_drvdata(pdev, wrp);

	wrp->master = of_device_get_match_data(&pdev->dev);
	wrp->slave = of_slave_id->data;
	wrp->dev = &pdev->dev;

	res = platform_get_resource_byname(pdev, IORESOURCE_MEM, "pwrap");
	wrp->base = devm_ioremap_resource(wrp->dev, res);
	if (IS_ERR(wrp->base))
		return PTR_ERR(wrp->base);

	if (HAS_CAP(wrp->master->caps, PWRAP_CAP_RESET)) {
		wrp->rstc = devm_reset_control_get(wrp->dev, "pwrap");
		if (IS_ERR(wrp->rstc)) {
			ret = PTR_ERR(wrp->rstc);
			dev_dbg(wrp->dev, "cannot get pwrap reset: %d\n", ret);
			return ret;
		}
	}

	if (HAS_CAP(wrp->master->caps, PWRAP_CAP_BRIDGE)) {
		res = platform_get_resource_byname(pdev, IORESOURCE_MEM,
				"pwrap-bridge");
		wrp->bridge_base = devm_ioremap_resource(wrp->dev, res);
		if (IS_ERR(wrp->bridge_base))
			return PTR_ERR(wrp->bridge_base);

		wrp->rstc_bridge = devm_reset_control_get(wrp->dev,
							  "pwrap-bridge");
		if (IS_ERR(wrp->rstc_bridge)) {
			ret = PTR_ERR(wrp->rstc_bridge);
			dev_dbg(wrp->dev,
				"cannot get pwrap-bridge reset: %d\n", ret);
			return ret;
		}
	}

	wrp->clk_spi = devm_clk_get(wrp->dev, "spi");
	if (IS_ERR(wrp->clk_spi)) {
		dev_dbg(wrp->dev, "failed to get clock: %ld\n",
			PTR_ERR(wrp->clk_spi));
		return PTR_ERR(wrp->clk_spi);
	}

	wrp->clk_wrap = devm_clk_get(wrp->dev, "wrap");
	if (IS_ERR(wrp->clk_wrap)) {
		dev_dbg(wrp->dev, "failed to get clock: %ld\n",
			PTR_ERR(wrp->clk_wrap));
		return PTR_ERR(wrp->clk_wrap);
	}

	ret = clk_prepare_enable(wrp->clk_spi);
	if (ret)
		return ret;

	ret = clk_prepare_enable(wrp->clk_wrap);
	if (ret)
		goto err_out1;

	/* Enable internal dynamic clock */
	if (HAS_CAP(wrp->master->caps, PWRAP_CAP_DCM)) {
		pwrap_writel(wrp, 1, PWRAP_DCM_EN);
		pwrap_writel(wrp, 0, PWRAP_DCM_DBC_PRD);
	}

	/*
	 * The PMIC could already be initialized by the bootloader.
	 * Skip initialization here in this case.
	 */
	if (!pwrap_readl(wrp, PWRAP_INIT_DONE2)) {
		ret = pwrap_init(wrp);
		if (ret) {
			dev_dbg(wrp->dev, "init failed with %d\n", ret);
			goto err_out2;
		}
	}

	if (HAS_CAP(wrp->master->caps, PWRAP_CAP_ARB))
		mask_done = PWRAP_STATE_INIT_DONE1;
	else
		mask_done = PWRAP_STATE_INIT_DONE0;

	if (!(pwrap_readl(wrp, PWRAP_WACS2_RDATA) & mask_done)) {
		dev_dbg(wrp->dev, "initialization isn't finished\n");
		ret = -ENODEV;
		goto err_out2;
	}

	/* Initialize watchdog, may not be done by the bootloader */
	if (!HAS_CAP(wrp->master->caps, PWRAP_CAP_ARB))
		pwrap_writel(wrp, 0xf, PWRAP_WDT_UNIT);

	/*
	 * Since STAUPD was not used on mt8173 platform,
	 * so STAUPD of WDT_SRC which should be turned off
	 */
	pwrap_writel(wrp, wrp->master->wdt_src, PWRAP_WDT_SRC_EN);
	if (HAS_CAP(wrp->master->caps, PWRAP_CAP_WDT_SRC1))
		pwrap_writel(wrp, wrp->master->wdt_src, PWRAP_WDT_SRC_EN_1);

	if (HAS_CAP(wrp->master->caps, PWRAP_CAP_ARB))
		pwrap_writel(wrp, 0x3, PWRAP_TIMER_EN);
	else
		pwrap_writel(wrp, 0x1, PWRAP_TIMER_EN);

	pwrap_writel(wrp, wrp->master->int_en_all, PWRAP_INT_EN);
	/*
	 * We add INT1 interrupt to handle starvation and request exception
	 * If we support it, we should enable it here.
	 */
	if (HAS_CAP(wrp->master->caps, PWRAP_CAP_INT1_EN))
		pwrap_writel(wrp, wrp->master->int1_en_all, PWRAP_INT1_EN);

	irq = platform_get_irq(pdev, 0);
	ret = devm_request_irq(wrp->dev, irq, pwrap_interrupt,
			       IRQF_TRIGGER_HIGH,
			       "mt-pmic-pwrap", wrp);
	if (ret)
		goto err_out2;

	wrp->regmap = devm_regmap_init(wrp->dev, NULL, wrp, wrp->slave->regmap);
	if (IS_ERR(wrp->regmap)) {
		ret = PTR_ERR(wrp->regmap);
		goto err_out2;
	}

	ret = of_platform_populate(np, NULL, NULL, wrp->dev);
	if (ret) {
		dev_dbg(wrp->dev, "failed to create child devices at %pOF\n",
				np);
		goto err_out2;
	}

	return 0;

err_out2:
	clk_disable_unprepare(wrp->clk_wrap);
err_out1:
	clk_disable_unprepare(wrp->clk_spi);

	return ret;
}