static int qcom_cpufreq_probe()

in qcom-cpufreq-nvmem.c [261:423]

• 145 lines of code
• 25 McCabe index (conditional complexity)

static int qcom_cpufreq_probe(struct platform_device *pdev)
{
	struct qcom_cpufreq_drv *drv;
	struct nvmem_cell *speedbin_nvmem;
	struct device_node *np;
	struct device *cpu_dev;
	char *pvs_name = "speedXX-pvsXX-vXX";
	unsigned cpu;
	const struct of_device_id *match;
	int ret;

	cpu_dev = get_cpu_device(0);
	if (!cpu_dev)
		return -ENODEV;

	np = dev_pm_opp_of_get_opp_desc_node(cpu_dev);
	if (!np)
		return -ENOENT;

	ret = of_device_is_compatible(np, "operating-points-v2-kryo-cpu");
	if (!ret) {
		of_node_put(np);
		return -ENOENT;
	}

	drv = kzalloc(sizeof(*drv), GFP_KERNEL);
	if (!drv)
		return -ENOMEM;

	match = pdev->dev.platform_data;
	drv->data = match->data;
	if (!drv->data) {
		ret = -ENODEV;
		goto free_drv;
	}

	if (drv->data->get_version) {
		speedbin_nvmem = of_nvmem_cell_get(np, NULL);
		if (IS_ERR(speedbin_nvmem)) {
			if (PTR_ERR(speedbin_nvmem) != -EPROBE_DEFER)
				dev_err(cpu_dev,
					"Could not get nvmem cell: %ld\n",
					PTR_ERR(speedbin_nvmem));
			ret = PTR_ERR(speedbin_nvmem);
			goto free_drv;
		}

		ret = drv->data->get_version(cpu_dev,
							speedbin_nvmem, &pvs_name, drv);
		if (ret) {
			nvmem_cell_put(speedbin_nvmem);
			goto free_drv;
		}
		nvmem_cell_put(speedbin_nvmem);
	}
	of_node_put(np);

	drv->names_opp_tables = kcalloc(num_possible_cpus(),
				  sizeof(*drv->names_opp_tables),
				  GFP_KERNEL);
	if (!drv->names_opp_tables) {
		ret = -ENOMEM;
		goto free_drv;
	}
	drv->hw_opp_tables = kcalloc(num_possible_cpus(),
				  sizeof(*drv->hw_opp_tables),
				  GFP_KERNEL);
	if (!drv->hw_opp_tables) {
		ret = -ENOMEM;
		goto free_opp_names;
	}

	drv->genpd_opp_tables = kcalloc(num_possible_cpus(),
					sizeof(*drv->genpd_opp_tables),
					GFP_KERNEL);
	if (!drv->genpd_opp_tables) {
		ret = -ENOMEM;
		goto free_opp;
	}

	for_each_possible_cpu(cpu) {
		cpu_dev = get_cpu_device(cpu);
		if (NULL == cpu_dev) {
			ret = -ENODEV;
			goto free_genpd_opp;
		}

		if (drv->data->get_version) {

			if (pvs_name) {
				drv->names_opp_tables[cpu] = dev_pm_opp_set_prop_name(
								     cpu_dev,
								     pvs_name);
				if (IS_ERR(drv->names_opp_tables[cpu])) {
					ret = PTR_ERR(drv->names_opp_tables[cpu]);
					dev_err(cpu_dev, "Failed to add OPP name %s\n",
						pvs_name);
					goto free_opp;
				}
			}

			drv->hw_opp_tables[cpu] = dev_pm_opp_set_supported_hw(
									 cpu_dev, &drv->versions, 1);
			if (IS_ERR(drv->hw_opp_tables[cpu])) {
				ret = PTR_ERR(drv->hw_opp_tables[cpu]);
				dev_err(cpu_dev,
					"Failed to set supported hardware\n");
				goto free_genpd_opp;
			}
		}

		if (drv->data->genpd_names) {
			drv->genpd_opp_tables[cpu] =
				dev_pm_opp_attach_genpd(cpu_dev,
							drv->data->genpd_names,
							NULL);
			if (IS_ERR(drv->genpd_opp_tables[cpu])) {
				ret = PTR_ERR(drv->genpd_opp_tables[cpu]);
				if (ret != -EPROBE_DEFER)
					dev_err(cpu_dev,
						"Could not attach to pm_domain: %d\n",
						ret);
				goto free_genpd_opp;
			}
		}
	}

	cpufreq_dt_pdev = platform_device_register_simple("cpufreq-dt", -1,
							  NULL, 0);
	if (!IS_ERR(cpufreq_dt_pdev)) {
		platform_set_drvdata(pdev, drv);
		return 0;
	}

	ret = PTR_ERR(cpufreq_dt_pdev);
	dev_err(cpu_dev, "Failed to register platform device\n");

free_genpd_opp:
	for_each_possible_cpu(cpu) {
		if (IS_ERR(drv->genpd_opp_tables[cpu]))
			break;
		dev_pm_opp_detach_genpd(drv->genpd_opp_tables[cpu]);
	}
	kfree(drv->genpd_opp_tables);
free_opp:
	for_each_possible_cpu(cpu) {
		if (IS_ERR(drv->names_opp_tables[cpu]))
			break;
		dev_pm_opp_put_prop_name(drv->names_opp_tables[cpu]);
	}
	for_each_possible_cpu(cpu) {
		if (IS_ERR(drv->hw_opp_tables[cpu]))
			break;
		dev_pm_opp_put_supported_hw(drv->hw_opp_tables[cpu]);
	}
	kfree(drv->hw_opp_tables);
free_opp_names:
	kfree(drv->names_opp_tables);
free_drv:
	kfree(drv);

	return ret;
}