static int __init armada37xx_cpufreq_driver_init()

in armada-37xx-cpufreq.c [403:532]

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

static int __init armada37xx_cpufreq_driver_init(void)
{
	struct cpufreq_dt_platform_data pdata;
	struct armada_37xx_dvfs *dvfs;
	struct platform_device *pdev;
	unsigned long freq;
	unsigned int base_frequency;
	struct regmap *nb_clk_base, *nb_pm_base, *avs_base;
	struct device *cpu_dev;
	int load_lvl, ret;
	struct clk *clk, *parent;

	nb_clk_base =
		syscon_regmap_lookup_by_compatible("marvell,armada-3700-periph-clock-nb");
	if (IS_ERR(nb_clk_base))
		return -ENODEV;

	nb_pm_base =
		syscon_regmap_lookup_by_compatible("marvell,armada-3700-nb-pm");

	if (IS_ERR(nb_pm_base))
		return -ENODEV;

	avs_base =
		syscon_regmap_lookup_by_compatible("marvell,armada-3700-avs");

	/* if AVS is not present don't use it but still try to setup dvfs */
	if (IS_ERR(avs_base)) {
		pr_info("Syscon failed for Adapting Voltage Scaling: skip it\n");
		avs_base = NULL;
	}
	/* Before doing any configuration on the DVFS first, disable it */
	armada37xx_cpufreq_disable_dvfs(nb_pm_base);

	/*
	 * On CPU 0 register the operating points supported (which are
	 * the nominal CPU frequency and full integer divisions of
	 * it).
	 */
	cpu_dev = get_cpu_device(0);
	if (!cpu_dev) {
		dev_err(cpu_dev, "Cannot get CPU\n");
		return -ENODEV;
	}

	clk = clk_get(cpu_dev, 0);
	if (IS_ERR(clk)) {
		dev_err(cpu_dev, "Cannot get clock for CPU0\n");
		return PTR_ERR(clk);
	}

	parent = clk_get_parent(clk);
	if (IS_ERR(parent)) {
		dev_err(cpu_dev, "Cannot get parent clock for CPU0\n");
		clk_put(clk);
		return PTR_ERR(parent);
	}

	/* Get parent CPU frequency */
	base_frequency =  clk_get_rate(parent);

	if (!base_frequency) {
		dev_err(cpu_dev, "Failed to get parent clock rate for CPU\n");
		clk_put(clk);
		return -EINVAL;
	}

	dvfs = armada_37xx_cpu_freq_info_get(base_frequency);
	if (!dvfs) {
		clk_put(clk);
		return -EINVAL;
	}

	armada37xx_cpufreq_state = kmalloc(sizeof(*armada37xx_cpufreq_state),
					   GFP_KERNEL);
	if (!armada37xx_cpufreq_state) {
		clk_put(clk);
		return -ENOMEM;
	}

	armada37xx_cpufreq_state->regmap = nb_pm_base;

	armada37xx_cpufreq_avs_configure(avs_base, dvfs);
	armada37xx_cpufreq_avs_setup(avs_base, dvfs);

	armada37xx_cpufreq_dvfs_setup(nb_pm_base, nb_clk_base, dvfs->divider);
	clk_put(clk);

	for (load_lvl = ARMADA_37XX_DVFS_LOAD_0; load_lvl < LOAD_LEVEL_NR;
	     load_lvl++) {
		unsigned long u_volt = avs_map[dvfs->avs[load_lvl]] * 1000;
		freq = base_frequency / dvfs->divider[load_lvl];
		ret = dev_pm_opp_add(cpu_dev, freq, u_volt);
		if (ret)
			goto remove_opp;


	}

	/* Now that everything is setup, enable the DVFS at hardware level */
	armada37xx_cpufreq_enable_dvfs(nb_pm_base);

	memset(&pdata, 0, sizeof(pdata));
	pdata.suspend = armada37xx_cpufreq_suspend;
	pdata.resume = armada37xx_cpufreq_resume;

	pdev = platform_device_register_data(NULL, "cpufreq-dt", -1, &pdata,
					     sizeof(pdata));
	ret = PTR_ERR_OR_ZERO(pdev);
	if (ret)
		goto disable_dvfs;

	armada37xx_cpufreq_state->cpu_dev = cpu_dev;
	armada37xx_cpufreq_state->pdev = pdev;
	platform_set_drvdata(pdev, dvfs);
	return 0;

disable_dvfs:
	armada37xx_cpufreq_disable_dvfs(nb_pm_base);
remove_opp:
	/* clean-up the already added opp before leaving */
	while (load_lvl-- > ARMADA_37XX_DVFS_LOAD_0) {
		freq = base_frequency / dvfs->divider[load_lvl];
		dev_pm_opp_remove(cpu_dev, freq);
	}

	kfree(armada37xx_cpufreq_state);

	return ret;
}