static int flexrm_mbox_probe()

in bcm-flexrm-mailbox.c [1495:1665]

• 133 lines of code
• 23 McCabe index (conditional complexity)

static int flexrm_mbox_probe(struct platform_device *pdev)
{
	int index, ret = 0;
	void __iomem *regs;
	void __iomem *regs_end;
	struct resource *iomem;
	struct flexrm_ring *ring;
	struct flexrm_mbox *mbox;
	struct device *dev = &pdev->dev;

	/* Allocate driver mailbox struct */
	mbox = devm_kzalloc(dev, sizeof(*mbox), GFP_KERNEL);
	if (!mbox) {
		ret = -ENOMEM;
		goto fail;
	}
	mbox->dev = dev;
	platform_set_drvdata(pdev, mbox);

	/* Get resource for registers */
	iomem = platform_get_resource(pdev, IORESOURCE_MEM, 0);
	if (!iomem || (resource_size(iomem) < RING_REGS_SIZE)) {
		ret = -ENODEV;
		goto fail;
	}

	/* Map registers of all rings */
	mbox->regs = devm_ioremap_resource(&pdev->dev, iomem);
	if (IS_ERR(mbox->regs)) {
		ret = PTR_ERR(mbox->regs);
		goto fail;
	}
	regs_end = mbox->regs + resource_size(iomem);

	/* Scan and count available rings */
	mbox->num_rings = 0;
	for (regs = mbox->regs; regs < regs_end; regs += RING_REGS_SIZE) {
		if (readl_relaxed(regs + RING_VER) == RING_VER_MAGIC)
			mbox->num_rings++;
	}
	if (!mbox->num_rings) {
		ret = -ENODEV;
		goto fail;
	}

	/* Allocate driver ring structs */
	ring = devm_kcalloc(dev, mbox->num_rings, sizeof(*ring), GFP_KERNEL);
	if (!ring) {
		ret = -ENOMEM;
		goto fail;
	}
	mbox->rings = ring;

	/* Initialize members of driver ring structs */
	regs = mbox->regs;
	for (index = 0; index < mbox->num_rings; index++) {
		ring = &mbox->rings[index];
		ring->num = index;
		ring->mbox = mbox;
		while ((regs < regs_end) &&
		       (readl_relaxed(regs + RING_VER) != RING_VER_MAGIC))
			regs += RING_REGS_SIZE;
		if (regs_end <= regs) {
			ret = -ENODEV;
			goto fail;
		}
		ring->regs = regs;
		regs += RING_REGS_SIZE;
		ring->irq = UINT_MAX;
		ring->irq_requested = false;
		ring->msi_timer_val = MSI_TIMER_VAL_MASK;
		ring->msi_count_threshold = 0x1;
		memset(ring->requests, 0, sizeof(ring->requests));
		ring->bd_base = NULL;
		ring->bd_dma_base = 0;
		ring->cmpl_base = NULL;
		ring->cmpl_dma_base = 0;
		atomic_set(&ring->msg_send_count, 0);
		atomic_set(&ring->msg_cmpl_count, 0);
		spin_lock_init(&ring->lock);
		bitmap_zero(ring->requests_bmap, RING_MAX_REQ_COUNT);
		ring->cmpl_read_offset = 0;
	}

	/* FlexRM is capable of 40-bit physical addresses only */
	ret = dma_set_mask_and_coherent(dev, DMA_BIT_MASK(40));
	if (ret) {
		ret = dma_set_mask_and_coherent(dev, DMA_BIT_MASK(32));
		if (ret)
			goto fail;
	}

	/* Create DMA pool for ring BD memory */
	mbox->bd_pool = dma_pool_create("bd", dev, RING_BD_SIZE,
					1 << RING_BD_ALIGN_ORDER, 0);
	if (!mbox->bd_pool) {
		ret = -ENOMEM;
		goto fail;
	}

	/* Create DMA pool for ring completion memory */
	mbox->cmpl_pool = dma_pool_create("cmpl", dev, RING_CMPL_SIZE,
					  1 << RING_CMPL_ALIGN_ORDER, 0);
	if (!mbox->cmpl_pool) {
		ret = -ENOMEM;
		goto fail_destroy_bd_pool;
	}

	/* Allocate platform MSIs for each ring */
	ret = platform_msi_domain_alloc_irqs(dev, mbox->num_rings,
						flexrm_mbox_msi_write);
	if (ret)
		goto fail_destroy_cmpl_pool;

	/* Save alloced IRQ numbers for each ring */
	for (index = 0; index < mbox->num_rings; index++)
		mbox->rings[index].irq = msi_get_virq(dev, index);

	/* Check availability of debugfs */
	if (!debugfs_initialized())
		goto skip_debugfs;

	/* Create debugfs root entry */
	mbox->root = debugfs_create_dir(dev_name(mbox->dev), NULL);

	/* Create debugfs config entry */
	debugfs_create_devm_seqfile(mbox->dev, "config", mbox->root,
				    flexrm_debugfs_conf_show);

	/* Create debugfs stats entry */
	debugfs_create_devm_seqfile(mbox->dev, "stats", mbox->root,
				    flexrm_debugfs_stats_show);

skip_debugfs:

	/* Initialize mailbox controller */
	mbox->controller.txdone_irq = false;
	mbox->controller.txdone_poll = false;
	mbox->controller.ops = &flexrm_mbox_chan_ops;
	mbox->controller.dev = dev;
	mbox->controller.num_chans = mbox->num_rings;
	mbox->controller.of_xlate = flexrm_mbox_of_xlate;
	mbox->controller.chans = devm_kcalloc(dev, mbox->num_rings,
				sizeof(*mbox->controller.chans), GFP_KERNEL);
	if (!mbox->controller.chans) {
		ret = -ENOMEM;
		goto fail_free_debugfs_root;
	}
	for (index = 0; index < mbox->num_rings; index++)
		mbox->controller.chans[index].con_priv = &mbox->rings[index];

	/* Register mailbox controller */
	ret = devm_mbox_controller_register(dev, &mbox->controller);
	if (ret)
		goto fail_free_debugfs_root;

	dev_info(dev, "registered flexrm mailbox with %d channels\n",
			mbox->controller.num_chans);

	return 0;

fail_free_debugfs_root:
	debugfs_remove_recursive(mbox->root);
	platform_msi_domain_free_irqs(dev);
fail_destroy_cmpl_pool:
	dma_pool_destroy(mbox->cmpl_pool);
fail_destroy_bd_pool:
	dma_pool_destroy(mbox->bd_pool);
fail:
	return ret;
}