static int mpc52xx_ata_probe()

in pata_mpc52xx.c [671:800]

• 94 lines of code
• 16 McCabe index (conditional complexity)

static int mpc52xx_ata_probe(struct platform_device *op)
{
	unsigned int ipb_freq;
	struct resource res_mem;
	int ata_irq = 0;
	struct mpc52xx_ata __iomem *ata_regs;
	struct mpc52xx_ata_priv *priv = NULL;
	int rv, task_irq;
	int mwdma_mask = 0, udma_mask = 0;
	const __be32 *prop;
	int proplen;
	struct bcom_task *dmatsk;

	/* Get ipb frequency */
	ipb_freq = mpc5xxx_get_bus_frequency(op->dev.of_node);
	if (!ipb_freq) {
		dev_err(&op->dev, "could not determine IPB bus frequency\n");
		return -ENODEV;
	}

	/* Get device base address from device tree, request the region
	 * and ioremap it. */
	rv = of_address_to_resource(op->dev.of_node, 0, &res_mem);
	if (rv) {
		dev_err(&op->dev, "could not determine device base address\n");
		return rv;
	}

	if (!devm_request_mem_region(&op->dev, res_mem.start,
				     sizeof(*ata_regs), DRV_NAME)) {
		dev_err(&op->dev, "error requesting register region\n");
		return -EBUSY;
	}

	ata_regs = devm_ioremap(&op->dev, res_mem.start, sizeof(*ata_regs));
	if (!ata_regs) {
		dev_err(&op->dev, "error mapping device registers\n");
		return -ENOMEM;
	}

	/*
	 * By default, all DMA modes are disabled for the MPC5200.  Some
	 * boards don't have the required signals routed to make DMA work.
	 * Also, the MPC5200B has a silicon bug that causes data corruption
	 * with UDMA if it is used at the same time as the LocalPlus bus.
	 *
	 * Instead of trying to guess what modes are usable, check the
	 * ATA device tree node to find out what DMA modes work on the board.
	 * UDMA/MWDMA modes can also be forced by adding "libata.force=<mode>"
	 * to the kernel boot parameters.
	 *
	 * The MPC5200 ATA controller supports MWDMA modes 0, 1 and 2 and
	 * UDMA modes 0, 1 and 2.
	 */
	prop = of_get_property(op->dev.of_node, "mwdma-mode", &proplen);
	if ((prop) && (proplen >= 4))
		mwdma_mask = ATA_MWDMA2 & ((1 << (*prop + 1)) - 1);
	prop = of_get_property(op->dev.of_node, "udma-mode", &proplen);
	if ((prop) && (proplen >= 4))
		udma_mask = ATA_UDMA2 & ((1 << (*prop + 1)) - 1);

	ata_irq = irq_of_parse_and_map(op->dev.of_node, 0);
	if (ata_irq == NO_IRQ) {
		dev_err(&op->dev, "error mapping irq\n");
		return -EINVAL;
	}

	/* Prepare our private structure */
	priv = devm_kzalloc(&op->dev, sizeof(*priv), GFP_ATOMIC);
	if (!priv) {
		rv = -ENOMEM;
		goto err1;
	}

	priv->ipb_period = 1000000000 / (ipb_freq / 1000);
	priv->ata_regs = ata_regs;
	priv->ata_regs_pa = res_mem.start;
	priv->ata_irq = ata_irq;
	priv->csel = -1;
	priv->mpc52xx_ata_dma_last_write = -1;

	if (ipb_freq/1000000 == 66) {
		priv->mdmaspec = mdmaspec66;
		priv->udmaspec = udmaspec66;
	} else {
		priv->mdmaspec = mdmaspec132;
		priv->udmaspec = udmaspec132;
	}

	/* Allocate a BestComm task for DMA */
	dmatsk = bcom_ata_init(MAX_DMA_BUFFERS, MAX_DMA_BUFFER_SIZE);
	if (!dmatsk) {
		dev_err(&op->dev, "bestcomm initialization failed\n");
		rv = -ENOMEM;
		goto err1;
	}

	task_irq = bcom_get_task_irq(dmatsk);
	rv = devm_request_irq(&op->dev, task_irq, &mpc52xx_ata_task_irq, 0,
				"ATA task", priv);
	if (rv) {
		dev_err(&op->dev, "error requesting DMA IRQ\n");
		goto err2;
	}
	priv->dmatsk = dmatsk;

	/* Init the hw */
	rv = mpc52xx_ata_hw_init(priv);
	if (rv) {
		dev_err(&op->dev, "error initializing hardware\n");
		goto err2;
	}

	/* Register ourselves to libata */
	rv = mpc52xx_ata_init_one(&op->dev, priv, res_mem.start,
				  mwdma_mask, udma_mask);
	if (rv) {
		dev_err(&op->dev, "error registering with ATA layer\n");
		goto err2;
	}

	return 0;

 err2:
	irq_dispose_mapping(task_irq);
	bcom_ata_release(dmatsk);
 err1:
	irq_dispose_mapping(ata_irq);
	return rv;
}