static int mv_platform_probe()

in sata_mv.c [4042:4203]

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

static int mv_platform_probe(struct platform_device *pdev)
{
	const struct mv_sata_platform_data *mv_platform_data;
	const struct mbus_dram_target_info *dram;
	const struct ata_port_info *ppi[] =
	    { &mv_port_info[chip_soc], NULL };
	struct ata_host *host;
	struct mv_host_priv *hpriv;
	struct resource *res;
	int n_ports = 0, irq = 0;
	int rc;
	int port;

	ata_print_version_once(&pdev->dev, DRV_VERSION);

	/*
	 * Simple resource validation ..
	 */
	if (unlikely(pdev->num_resources != 2)) {
		dev_err(&pdev->dev, "invalid number of resources\n");
		return -EINVAL;
	}

	/*
	 * Get the register base first
	 */
	res = platform_get_resource(pdev, IORESOURCE_MEM, 0);
	if (res == NULL)
		return -EINVAL;

	/* allocate host */
	if (pdev->dev.of_node) {
		rc = of_property_read_u32(pdev->dev.of_node, "nr-ports",
					   &n_ports);
		if (rc) {
			dev_err(&pdev->dev,
				"error parsing nr-ports property: %d\n", rc);
			return rc;
		}

		if (n_ports <= 0) {
			dev_err(&pdev->dev, "nr-ports must be positive: %d\n",
				n_ports);
			return -EINVAL;
		}

		irq = irq_of_parse_and_map(pdev->dev.of_node, 0);
	} else {
		mv_platform_data = dev_get_platdata(&pdev->dev);
		n_ports = mv_platform_data->n_ports;
		irq = platform_get_irq(pdev, 0);
	}
	if (irq < 0)
		return irq;
	if (!irq)
		return -EINVAL;

	host = ata_host_alloc_pinfo(&pdev->dev, ppi, n_ports);
	hpriv = devm_kzalloc(&pdev->dev, sizeof(*hpriv), GFP_KERNEL);

	if (!host || !hpriv)
		return -ENOMEM;
	hpriv->port_clks = devm_kcalloc(&pdev->dev,
					n_ports, sizeof(struct clk *),
					GFP_KERNEL);
	if (!hpriv->port_clks)
		return -ENOMEM;
	hpriv->port_phys = devm_kcalloc(&pdev->dev,
					n_ports, sizeof(struct phy *),
					GFP_KERNEL);
	if (!hpriv->port_phys)
		return -ENOMEM;
	host->private_data = hpriv;
	hpriv->board_idx = chip_soc;

	host->iomap = NULL;
	hpriv->base = devm_ioremap(&pdev->dev, res->start,
				   resource_size(res));
	if (!hpriv->base)
		return -ENOMEM;

	hpriv->base -= SATAHC0_REG_BASE;

	hpriv->clk = clk_get(&pdev->dev, NULL);
	if (IS_ERR(hpriv->clk))
		dev_notice(&pdev->dev, "cannot get optional clkdev\n");
	else
		clk_prepare_enable(hpriv->clk);

	for (port = 0; port < n_ports; port++) {
		char port_number[16];
		sprintf(port_number, "%d", port);
		hpriv->port_clks[port] = clk_get(&pdev->dev, port_number);
		if (!IS_ERR(hpriv->port_clks[port]))
			clk_prepare_enable(hpriv->port_clks[port]);

		sprintf(port_number, "port%d", port);
		hpriv->port_phys[port] = devm_phy_optional_get(&pdev->dev,
							       port_number);
		if (IS_ERR(hpriv->port_phys[port])) {
			rc = PTR_ERR(hpriv->port_phys[port]);
			hpriv->port_phys[port] = NULL;
			if (rc != -EPROBE_DEFER)
				dev_warn(&pdev->dev, "error getting phy %d", rc);

			/* Cleanup only the initialized ports */
			hpriv->n_ports = port;
			goto err;
		} else
			phy_power_on(hpriv->port_phys[port]);
	}

	/* All the ports have been initialized */
	hpriv->n_ports = n_ports;

	/*
	 * (Re-)program MBUS remapping windows if we are asked to.
	 */
	dram = mv_mbus_dram_info();
	if (dram)
		mv_conf_mbus_windows(hpriv, dram);

	rc = mv_create_dma_pools(hpriv, &pdev->dev);
	if (rc)
		goto err;

	/*
	 * To allow disk hotplug on Armada 370/XP SoCs, the PHY speed must be
	 * updated in the LP_PHY_CTL register.
	 */
	if (pdev->dev.of_node &&
		of_device_is_compatible(pdev->dev.of_node,
					"marvell,armada-370-sata"))
		hpriv->hp_flags |= MV_HP_FIX_LP_PHY_CTL;

	/* initialize adapter */
	rc = mv_init_host(host);
	if (rc)
		goto err;

	dev_info(&pdev->dev, "slots %u ports %d\n",
		 (unsigned)MV_MAX_Q_DEPTH, host->n_ports);

	rc = ata_host_activate(host, irq, mv_interrupt, IRQF_SHARED, &mv6_sht);
	if (!rc)
		return 0;

err:
	if (!IS_ERR(hpriv->clk)) {
		clk_disable_unprepare(hpriv->clk);
		clk_put(hpriv->clk);
	}
	for (port = 0; port < hpriv->n_ports; port++) {
		if (!IS_ERR(hpriv->port_clks[port])) {
			clk_disable_unprepare(hpriv->port_clks[port]);
			clk_put(hpriv->port_clks[port]);
		}
		phy_power_off(hpriv->port_phys[port]);
	}

	return rc;
}