static int __init sunxi_mc_smp_init()

in mach-sunxi/mc_smp.c [780:912]

• 95 lines of code
• 16 McCabe index (conditional complexity)

static int __init sunxi_mc_smp_init(void)
{
	struct sunxi_mc_smp_nodes nodes = { 0 };
	struct device_node *node;
	struct resource res;
	void __iomem *addr;
	int i, ret;

	/*
	 * Don't bother checking the "cpus" node, as an enable-method
	 * property in that node is undocumented.
	 */
	node = of_cpu_device_node_get(0);
	if (!node)
		return -ENODEV;

	/*
	 * We can't actually use the enable-method magic in the kernel.
	 * Our loopback / trampoline code uses the CPU suspend framework,
	 * which requires the identity mapping be available. It would not
	 * yet be available if we used the .init_cpus or .prepare_cpus
	 * callbacks in smp_operations, which we would use if we were to
	 * use CPU_METHOD_OF_DECLARE
	 */
	for (i = 0; i < ARRAY_SIZE(sunxi_mc_smp_data); i++) {
		ret = of_property_match_string(node, "enable-method",
					       sunxi_mc_smp_data[i].enable_method);
		if (!ret)
			break;
	}

	is_a83t = sunxi_mc_smp_data[i].is_a83t;

	of_node_put(node);
	if (ret)
		return -ENODEV;

	if (!sunxi_mc_smp_cpu_table_init())
		return -EINVAL;

	if (!cci_probed()) {
		pr_err("%s: CCI-400 not available\n", __func__);
		return -ENODEV;
	}

	/* Get needed device tree nodes */
	ret = sunxi_mc_smp_data[i].get_smp_nodes(&nodes);
	if (ret)
		goto err_put_nodes;

	/*
	 * Unfortunately we can not request the I/O region for the PRCM.
	 * It is shared with the PRCM clock.
	 */
	prcm_base = of_iomap(nodes.prcm_node, 0);
	if (!prcm_base) {
		pr_err("%s: failed to map PRCM registers\n", __func__);
		ret = -ENOMEM;
		goto err_put_nodes;
	}

	cpucfg_base = of_io_request_and_map(nodes.cpucfg_node, 0,
					    "sunxi-mc-smp");
	if (IS_ERR(cpucfg_base)) {
		ret = PTR_ERR(cpucfg_base);
		pr_err("%s: failed to map CPUCFG registers: %d\n",
		       __func__, ret);
		goto err_unmap_prcm;
	}

	if (is_a83t) {
		r_cpucfg_base = of_io_request_and_map(nodes.r_cpucfg_node,
						      0, "sunxi-mc-smp");
		if (IS_ERR(r_cpucfg_base)) {
			ret = PTR_ERR(r_cpucfg_base);
			pr_err("%s: failed to map R-CPUCFG registers\n",
			       __func__);
			goto err_unmap_release_cpucfg;
		}
	} else {
		sram_b_smp_base = of_io_request_and_map(nodes.sram_node, 0,
							"sunxi-mc-smp");
		if (IS_ERR(sram_b_smp_base)) {
			ret = PTR_ERR(sram_b_smp_base);
			pr_err("%s: failed to map secure SRAM\n", __func__);
			goto err_unmap_release_cpucfg;
		}
	}

	/* Configure CCI-400 for boot cluster */
	ret = sunxi_mc_smp_loopback();
	if (ret) {
		pr_err("%s: failed to configure boot cluster: %d\n",
		       __func__, ret);
		goto err_unmap_release_sram_rcpucfg;
	}

	/* We don't need the device nodes anymore */
	sunxi_mc_smp_put_nodes(&nodes);

	/* Set the hardware entry point address */
	if (is_a83t)
		addr = r_cpucfg_base + R_CPUCFG_CPU_SOFT_ENTRY_REG;
	else
		addr = prcm_base + PRCM_CPU_SOFT_ENTRY_REG;
	writel(__pa_symbol(sunxi_mc_smp_secondary_startup), addr);

	/* Actually enable multi cluster SMP */
	smp_set_ops(&sunxi_mc_smp_smp_ops);

	pr_info("sunxi multi cluster SMP support installed\n");

	return 0;

err_unmap_release_sram_rcpucfg:
	if (is_a83t) {
		iounmap(r_cpucfg_base);
		of_address_to_resource(nodes.r_cpucfg_node, 0, &res);
	} else {
		iounmap(sram_b_smp_base);
		of_address_to_resource(nodes.sram_node, 0, &res);
	}
	release_mem_region(res.start, resource_size(&res));
err_unmap_release_cpucfg:
	iounmap(cpucfg_base);
	of_address_to_resource(nodes.cpucfg_node, 0, &res);
	release_mem_region(res.start, resource_size(&res));
err_unmap_prcm:
	iounmap(prcm_base);
err_put_nodes:
	sunxi_mc_smp_put_nodes(&nodes);
	return ret;
}