static int qcom_smsm_probe()

in qcom/smsm.c [475:608]

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

static int qcom_smsm_probe(struct platform_device *pdev)
{
	struct device_node *local_node;
	struct device_node *node;
	struct smsm_entry *entry;
	struct qcom_smsm *smsm;
	u32 *intr_mask;
	size_t size;
	u32 *states;
	u32 id;
	int ret;

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

	ret = smsm_get_size_info(smsm);
	if (ret)
		return ret;

	smsm->entries = devm_kcalloc(&pdev->dev,
				     smsm->num_entries,
				     sizeof(struct smsm_entry),
				     GFP_KERNEL);
	if (!smsm->entries)
		return -ENOMEM;

	smsm->hosts = devm_kcalloc(&pdev->dev,
				   smsm->num_hosts,
				   sizeof(struct smsm_host),
				   GFP_KERNEL);
	if (!smsm->hosts)
		return -ENOMEM;

	for_each_child_of_node(pdev->dev.of_node, local_node) {
		if (of_find_property(local_node, "#qcom,smem-state-cells", NULL))
			break;
	}
	if (!local_node) {
		dev_err(&pdev->dev, "no state entry\n");
		return -EINVAL;
	}

	of_property_read_u32(pdev->dev.of_node,
			     "qcom,local-host",
			     &smsm->local_host);

	/* Parse the host properties */
	for (id = 0; id < smsm->num_hosts; id++) {
		ret = smsm_parse_ipc(smsm, id);
		if (ret < 0)
			return ret;
	}

	/* Acquire the main SMSM state vector */
	ret = qcom_smem_alloc(QCOM_SMEM_HOST_ANY, SMEM_SMSM_SHARED_STATE,
			      smsm->num_entries * sizeof(u32));
	if (ret < 0 && ret != -EEXIST) {
		dev_err(&pdev->dev, "unable to allocate shared state entry\n");
		return ret;
	}

	states = qcom_smem_get(QCOM_SMEM_HOST_ANY, SMEM_SMSM_SHARED_STATE, NULL);
	if (IS_ERR(states)) {
		dev_err(&pdev->dev, "Unable to acquire shared state entry\n");
		return PTR_ERR(states);
	}

	/* Acquire the list of interrupt mask vectors */
	size = smsm->num_entries * smsm->num_hosts * sizeof(u32);
	ret = qcom_smem_alloc(QCOM_SMEM_HOST_ANY, SMEM_SMSM_CPU_INTR_MASK, size);
	if (ret < 0 && ret != -EEXIST) {
		dev_err(&pdev->dev, "unable to allocate smsm interrupt mask\n");
		return ret;
	}

	intr_mask = qcom_smem_get(QCOM_SMEM_HOST_ANY, SMEM_SMSM_CPU_INTR_MASK, NULL);
	if (IS_ERR(intr_mask)) {
		dev_err(&pdev->dev, "unable to acquire shared memory interrupt mask\n");
		return PTR_ERR(intr_mask);
	}

	/* Setup the reference to the local state bits */
	smsm->local_state = states + smsm->local_host;
	smsm->subscription = intr_mask + smsm->local_host * smsm->num_hosts;

	/* Register the outgoing state */
	smsm->state = qcom_smem_state_register(local_node, &smsm_state_ops, smsm);
	if (IS_ERR(smsm->state)) {
		dev_err(smsm->dev, "failed to register qcom_smem_state\n");
		return PTR_ERR(smsm->state);
	}

	/* Register handlers for remote processor entries of interest. */
	for_each_available_child_of_node(pdev->dev.of_node, node) {
		if (!of_property_read_bool(node, "interrupt-controller"))
			continue;

		ret = of_property_read_u32(node, "reg", &id);
		if (ret || id >= smsm->num_entries) {
			dev_err(&pdev->dev, "invalid reg of entry\n");
			if (!ret)
				ret = -EINVAL;
			goto unwind_interfaces;
		}
		entry = &smsm->entries[id];

		entry->smsm = smsm;
		entry->remote_state = states + id;

		/* Setup subscription pointers and unsubscribe to any kicks */
		entry->subscription = intr_mask + id * smsm->num_hosts;
		writel(0, entry->subscription + smsm->local_host);

		ret = smsm_inbound_entry(smsm, entry, node);
		if (ret < 0)
			goto unwind_interfaces;
	}

	platform_set_drvdata(pdev, smsm);

	return 0;

unwind_interfaces:
	for (id = 0; id < smsm->num_entries; id++)
		if (smsm->entries[id].domain)
			irq_domain_remove(smsm->entries[id].domain);

	qcom_smem_state_unregister(smsm->state);

	return ret;
}