static int scpi_probe()

in arm_scpi.c [910:1028]

• 98 lines of code
• 19 McCabe index (conditional complexity)

static int scpi_probe(struct platform_device *pdev)
{
	int count, idx, ret;
	struct resource res;
	struct device *dev = &pdev->dev;
	struct device_node *np = dev->of_node;

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

	if (of_match_device(legacy_scpi_of_match, &pdev->dev))
		scpi_info->is_legacy = true;

	count = of_count_phandle_with_args(np, "mboxes", "#mbox-cells");
	if (count < 0) {
		dev_err(dev, "no mboxes property in '%pOF'\n", np);
		return -ENODEV;
	}

	scpi_info->channels = devm_kcalloc(dev, count, sizeof(struct scpi_chan),
					   GFP_KERNEL);
	if (!scpi_info->channels)
		return -ENOMEM;

	ret = devm_add_action(dev, scpi_free_channels, scpi_info);
	if (ret)
		return ret;

	for (; scpi_info->num_chans < count; scpi_info->num_chans++) {
		resource_size_t size;
		int idx = scpi_info->num_chans;
		struct scpi_chan *pchan = scpi_info->channels + idx;
		struct mbox_client *cl = &pchan->cl;
		struct device_node *shmem = of_parse_phandle(np, "shmem", idx);

		if (!of_match_node(shmem_of_match, shmem))
			return -ENXIO;

		ret = of_address_to_resource(shmem, 0, &res);
		of_node_put(shmem);
		if (ret) {
			dev_err(dev, "failed to get SCPI payload mem resource\n");
			return ret;
		}

		size = resource_size(&res);
		pchan->rx_payload = devm_ioremap(dev, res.start, size);
		if (!pchan->rx_payload) {
			dev_err(dev, "failed to ioremap SCPI payload\n");
			return -EADDRNOTAVAIL;
		}
		pchan->tx_payload = pchan->rx_payload + (size >> 1);

		cl->dev = dev;
		cl->rx_callback = scpi_handle_remote_msg;
		cl->tx_prepare = scpi_tx_prepare;
		cl->tx_block = true;
		cl->tx_tout = 20;
		cl->knows_txdone = false; /* controller can't ack */

		INIT_LIST_HEAD(&pchan->rx_pending);
		INIT_LIST_HEAD(&pchan->xfers_list);
		spin_lock_init(&pchan->rx_lock);
		mutex_init(&pchan->xfers_lock);

		ret = scpi_alloc_xfer_list(dev, pchan);
		if (!ret) {
			pchan->chan = mbox_request_channel(cl, idx);
			if (!IS_ERR(pchan->chan))
				continue;
			ret = PTR_ERR(pchan->chan);
			if (ret != -EPROBE_DEFER)
				dev_err(dev, "failed to get channel%d err %d\n",
					idx, ret);
		}
		return ret;
	}

	scpi_info->commands = scpi_std_commands;

	platform_set_drvdata(pdev, scpi_info);

	if (scpi_info->is_legacy) {
		/* Replace with legacy variants */
		scpi_ops.clk_set_val = legacy_scpi_clk_set_val;
		scpi_info->commands = scpi_legacy_commands;

		/* Fill priority bitmap */
		for (idx = 0; idx < ARRAY_SIZE(legacy_hpriority_cmds); idx++)
			set_bit(legacy_hpriority_cmds[idx],
				scpi_info->cmd_priority);
	}

	ret = scpi_init_versions(scpi_info);
	if (ret) {
		dev_err(dev, "incorrect or no SCP firmware found\n");
		return ret;
	}

	if (scpi_info->is_legacy && !scpi_info->protocol_version &&
	    !scpi_info->firmware_version)
		dev_info(dev, "SCP Protocol legacy pre-1.0 firmware\n");
	else
		dev_info(dev, "SCP Protocol %lu.%lu Firmware %lu.%lu.%lu version\n",
			 FIELD_GET(PROTO_REV_MAJOR_MASK,
				   scpi_info->protocol_version),
			 FIELD_GET(PROTO_REV_MINOR_MASK,
				   scpi_info->protocol_version),
			 FIELD_GET(FW_REV_MAJOR_MASK,
				   scpi_info->firmware_version),
			 FIELD_GET(FW_REV_MINOR_MASK,
				   scpi_info->firmware_version),
			 FIELD_GET(FW_REV_PATCH_MASK,
				   scpi_info->firmware_version));
	scpi_info->scpi_ops = &scpi_ops;

	return devm_of_platform_populate(dev);
}