static int cmdq_probe()

in mtk-cmdq-mailbox.c [524:632]

• 91 lines of code
• 15 McCabe index (conditional complexity)

static int cmdq_probe(struct platform_device *pdev)
{
	struct device *dev = &pdev->dev;
	struct cmdq *cmdq;
	int err, i;
	struct gce_plat *plat_data;
	struct device_node *phandle = dev->of_node;
	struct device_node *node;
	int alias_id = 0;
	static const char * const clk_name = "gce";
	static const char * const clk_names[] = { "gce0", "gce1" };

	cmdq = devm_kzalloc(dev, sizeof(*cmdq), GFP_KERNEL);
	if (!cmdq)
		return -ENOMEM;

	cmdq->base = devm_platform_ioremap_resource(pdev, 0);
	if (IS_ERR(cmdq->base))
		return PTR_ERR(cmdq->base);

	cmdq->irq = platform_get_irq(pdev, 0);
	if (cmdq->irq < 0)
		return cmdq->irq;

	plat_data = (struct gce_plat *)of_device_get_match_data(dev);
	if (!plat_data) {
		dev_err(dev, "failed to get match data\n");
		return -EINVAL;
	}

	cmdq->thread_nr = plat_data->thread_nr;
	cmdq->shift_pa = plat_data->shift;
	cmdq->control_by_sw = plat_data->control_by_sw;
	cmdq->gce_num = plat_data->gce_num;
	cmdq->irq_mask = GENMASK(cmdq->thread_nr - 1, 0);
	err = devm_request_irq(dev, cmdq->irq, cmdq_irq_handler, IRQF_SHARED,
			       "mtk_cmdq", cmdq);
	if (err < 0) {
		dev_err(dev, "failed to register ISR (%d)\n", err);
		return err;
	}

	dev_dbg(dev, "cmdq device: addr:0x%p, va:0x%p, irq:%d\n",
		dev, cmdq->base, cmdq->irq);

	if (cmdq->gce_num > 1) {
		for_each_child_of_node(phandle->parent, node) {
			alias_id = of_alias_get_id(node, clk_name);
			if (alias_id >= 0 && alias_id < cmdq->gce_num) {
				cmdq->clocks[alias_id].id = clk_names[alias_id];
				cmdq->clocks[alias_id].clk = of_clk_get(node, 0);
				if (IS_ERR(cmdq->clocks[alias_id].clk)) {
					of_node_put(node);
					return dev_err_probe(dev,
							     PTR_ERR(cmdq->clocks[alias_id].clk),
							     "failed to get gce clk: %d\n",
							     alias_id);
				}
			}
		}
	} else {
		cmdq->clocks[alias_id].id = clk_name;
		cmdq->clocks[alias_id].clk = devm_clk_get(&pdev->dev, clk_name);
		if (IS_ERR(cmdq->clocks[alias_id].clk)) {
			return dev_err_probe(dev, PTR_ERR(cmdq->clocks[alias_id].clk),
					     "failed to get gce clk\n");
		}
	}

	cmdq->mbox.dev = dev;
	cmdq->mbox.chans = devm_kcalloc(dev, cmdq->thread_nr,
					sizeof(*cmdq->mbox.chans), GFP_KERNEL);
	if (!cmdq->mbox.chans)
		return -ENOMEM;

	cmdq->mbox.num_chans = cmdq->thread_nr;
	cmdq->mbox.ops = &cmdq_mbox_chan_ops;
	cmdq->mbox.of_xlate = cmdq_xlate;

	/* make use of TXDONE_BY_ACK */
	cmdq->mbox.txdone_irq = false;
	cmdq->mbox.txdone_poll = false;

	cmdq->thread = devm_kcalloc(dev, cmdq->thread_nr,
					sizeof(*cmdq->thread), GFP_KERNEL);
	if (!cmdq->thread)
		return -ENOMEM;

	for (i = 0; i < cmdq->thread_nr; i++) {
		cmdq->thread[i].base = cmdq->base + CMDQ_THR_BASE +
				CMDQ_THR_SIZE * i;
		INIT_LIST_HEAD(&cmdq->thread[i].task_busy_list);
		cmdq->mbox.chans[i].con_priv = (void *)&cmdq->thread[i];
	}

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

	platform_set_drvdata(pdev, cmdq);

	WARN_ON(clk_bulk_prepare(cmdq->gce_num, cmdq->clocks));

	cmdq_init(cmdq);

	return 0;
}