static int tegra_hsp_probe()

in tegra-hsp.c [640:784]

• 118 lines of code
• 22 McCabe index (conditional complexity)

static int tegra_hsp_probe(struct platform_device *pdev)
{
	struct tegra_hsp *hsp;
	struct resource *res;
	unsigned int i;
	u32 value;
	int err;

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

	hsp->dev = &pdev->dev;
	hsp->soc = of_device_get_match_data(&pdev->dev);
	INIT_LIST_HEAD(&hsp->doorbells);
	spin_lock_init(&hsp->lock);

	res = platform_get_resource(pdev, IORESOURCE_MEM, 0);
	hsp->regs = devm_ioremap_resource(&pdev->dev, res);
	if (IS_ERR(hsp->regs))
		return PTR_ERR(hsp->regs);

	value = tegra_hsp_readl(hsp, HSP_INT_DIMENSIONING);
	hsp->num_sm = (value >> HSP_nSM_SHIFT) & HSP_nINT_MASK;
	hsp->num_ss = (value >> HSP_nSS_SHIFT) & HSP_nINT_MASK;
	hsp->num_as = (value >> HSP_nAS_SHIFT) & HSP_nINT_MASK;
	hsp->num_db = (value >> HSP_nDB_SHIFT) & HSP_nINT_MASK;
	hsp->num_si = (value >> HSP_nSI_SHIFT) & HSP_nINT_MASK;

	err = platform_get_irq_byname_optional(pdev, "doorbell");
	if (err >= 0)
		hsp->doorbell_irq = err;

	if (hsp->num_si > 0) {
		unsigned int count = 0;

		hsp->shared_irqs = devm_kcalloc(&pdev->dev, hsp->num_si,
						sizeof(*hsp->shared_irqs),
						GFP_KERNEL);
		if (!hsp->shared_irqs)
			return -ENOMEM;

		for (i = 0; i < hsp->num_si; i++) {
			char *name;

			name = kasprintf(GFP_KERNEL, "shared%u", i);
			if (!name)
				return -ENOMEM;

			err = platform_get_irq_byname_optional(pdev, name);
			if (err >= 0) {
				hsp->shared_irqs[i] = err;
				count++;
			}

			kfree(name);
		}

		if (count == 0) {
			devm_kfree(&pdev->dev, hsp->shared_irqs);
			hsp->shared_irqs = NULL;
		}
	}

	/* setup the doorbell controller */
	hsp->mbox_db.of_xlate = tegra_hsp_db_xlate;
	hsp->mbox_db.num_chans = 32;
	hsp->mbox_db.dev = &pdev->dev;
	hsp->mbox_db.ops = &tegra_hsp_db_ops;

	hsp->mbox_db.chans = devm_kcalloc(&pdev->dev, hsp->mbox_db.num_chans,
					  sizeof(*hsp->mbox_db.chans),
					  GFP_KERNEL);
	if (!hsp->mbox_db.chans)
		return -ENOMEM;

	if (hsp->doorbell_irq) {
		err = tegra_hsp_add_doorbells(hsp);
		if (err < 0) {
			dev_err(&pdev->dev, "failed to add doorbells: %d\n",
			        err);
			return err;
		}
	}

	err = devm_mbox_controller_register(&pdev->dev, &hsp->mbox_db);
	if (err < 0) {
		dev_err(&pdev->dev, "failed to register doorbell mailbox: %d\n",
			err);
		return err;
	}

	/* setup the shared mailbox controller */
	hsp->mbox_sm.of_xlate = tegra_hsp_sm_xlate;
	hsp->mbox_sm.num_chans = hsp->num_sm;
	hsp->mbox_sm.dev = &pdev->dev;
	hsp->mbox_sm.ops = &tegra_hsp_sm_ops;

	hsp->mbox_sm.chans = devm_kcalloc(&pdev->dev, hsp->mbox_sm.num_chans,
					  sizeof(*hsp->mbox_sm.chans),
					  GFP_KERNEL);
	if (!hsp->mbox_sm.chans)
		return -ENOMEM;

	if (hsp->shared_irqs) {
		err = tegra_hsp_add_mailboxes(hsp, &pdev->dev);
		if (err < 0) {
			dev_err(&pdev->dev, "failed to add mailboxes: %d\n",
			        err);
			return err;
		}
	}

	err = devm_mbox_controller_register(&pdev->dev, &hsp->mbox_sm);
	if (err < 0) {
		dev_err(&pdev->dev, "failed to register shared mailbox: %d\n",
			err);
		return err;
	}

	platform_set_drvdata(pdev, hsp);

	if (hsp->doorbell_irq) {
		err = devm_request_irq(&pdev->dev, hsp->doorbell_irq,
				       tegra_hsp_doorbell_irq, IRQF_NO_SUSPEND,
				       dev_name(&pdev->dev), hsp);
		if (err < 0) {
			dev_err(&pdev->dev,
			        "failed to request doorbell IRQ#%u: %d\n",
				hsp->doorbell_irq, err);
			return err;
		}
	}

	if (hsp->shared_irqs) {
		err = tegra_hsp_request_shared_irq(hsp);
		if (err < 0)
			return err;
	}

	lockdep_register_key(&hsp->lock_key);
	lockdep_set_class(&hsp->lock, &hsp->lock_key);

	return 0;
}