static int __init imx6q_suspend_init()

in mach-imx/pm-imx6.c [463:593]

• 105 lines of code
• 13 McCabe index (conditional complexity)

static int __init imx6q_suspend_init(const struct imx6_pm_socdata *socdata)
{
	phys_addr_t ocram_pbase;
	struct device_node *node;
	struct platform_device *pdev;
	struct imx6_cpu_pm_info *pm_info;
	struct gen_pool *ocram_pool;
	unsigned long ocram_base;
	int i, ret = 0;
	const u32 *mmdc_offset_array;

	suspend_set_ops(&imx6q_pm_ops);

	if (!socdata) {
		pr_warn("%s: invalid argument!\n", __func__);
		return -EINVAL;
	}

	node = of_find_compatible_node(NULL, NULL, "mmio-sram");
	if (!node) {
		pr_warn("%s: failed to find ocram node!\n", __func__);
		return -ENODEV;
	}

	pdev = of_find_device_by_node(node);
	if (!pdev) {
		pr_warn("%s: failed to find ocram device!\n", __func__);
		ret = -ENODEV;
		goto put_node;
	}

	ocram_pool = gen_pool_get(&pdev->dev, NULL);
	if (!ocram_pool) {
		pr_warn("%s: ocram pool unavailable!\n", __func__);
		ret = -ENODEV;
		goto put_device;
	}

	ocram_base = gen_pool_alloc(ocram_pool, MX6Q_SUSPEND_OCRAM_SIZE);
	if (!ocram_base) {
		pr_warn("%s: unable to alloc ocram!\n", __func__);
		ret = -ENOMEM;
		goto put_device;
	}

	ocram_pbase = gen_pool_virt_to_phys(ocram_pool, ocram_base);

	suspend_ocram_base = __arm_ioremap_exec(ocram_pbase,
		MX6Q_SUSPEND_OCRAM_SIZE, false);

	memset(suspend_ocram_base, 0, sizeof(*pm_info));
	pm_info = suspend_ocram_base;
	pm_info->pbase = ocram_pbase;
	pm_info->resume_addr = __pa_symbol(v7_cpu_resume);
	pm_info->pm_info_size = sizeof(*pm_info);

	/*
	 * ccm physical address is not used by asm code currently,
	 * so get ccm virtual address directly.
	 */
	pm_info->ccm_base.vbase = ccm_base;

	ret = imx6_pm_get_base(&pm_info->mmdc_base, socdata->mmdc_compat);
	if (ret) {
		pr_warn("%s: failed to get mmdc base %d!\n", __func__, ret);
		goto put_device;
	}

	ret = imx6_pm_get_base(&pm_info->src_base, socdata->src_compat);
	if (ret) {
		pr_warn("%s: failed to get src base %d!\n", __func__, ret);
		goto src_map_failed;
	}

	ret = imx6_pm_get_base(&pm_info->iomuxc_base, socdata->iomuxc_compat);
	if (ret) {
		pr_warn("%s: failed to get iomuxc base %d!\n", __func__, ret);
		goto iomuxc_map_failed;
	}

	ret = imx6_pm_get_base(&pm_info->gpc_base, socdata->gpc_compat);
	if (ret) {
		pr_warn("%s: failed to get gpc base %d!\n", __func__, ret);
		goto gpc_map_failed;
	}

	if (socdata->pl310_compat) {
		ret = imx6_pm_get_base(&pm_info->l2_base, socdata->pl310_compat);
		if (ret) {
			pr_warn("%s: failed to get pl310-cache base %d!\n",
				__func__, ret);
			goto pl310_cache_map_failed;
		}
	}

	pm_info->ddr_type = imx_mmdc_get_ddr_type();
	pm_info->mmdc_io_num = socdata->mmdc_io_num;
	mmdc_offset_array = socdata->mmdc_io_offset;

	for (i = 0; i < pm_info->mmdc_io_num; i++) {
		pm_info->mmdc_io_val[i][0] =
			mmdc_offset_array[i];
		pm_info->mmdc_io_val[i][1] =
			readl_relaxed(pm_info->iomuxc_base.vbase +
			mmdc_offset_array[i]);
	}

	imx6_suspend_in_ocram_fn = fncpy(
		suspend_ocram_base + sizeof(*pm_info),
		&imx6_suspend,
		MX6Q_SUSPEND_OCRAM_SIZE - sizeof(*pm_info));

	__arm_iomem_set_ro(suspend_ocram_base, MX6Q_SUSPEND_OCRAM_SIZE);

	goto put_device;

pl310_cache_map_failed:
	iounmap(pm_info->gpc_base.vbase);
gpc_map_failed:
	iounmap(pm_info->iomuxc_base.vbase);
iomuxc_map_failed:
	iounmap(pm_info->src_base.vbase);
src_map_failed:
	iounmap(pm_info->mmdc_base.vbase);
put_device:
	put_device(&pdev->dev);
put_node:
	of_node_put(node);

	return ret;
}