cpufreq-cpu0.c 8.8 KB
Newer Older
1 2 3
/*
 * Copyright (C) 2012 Freescale Semiconductor, Inc.
 *
4 5 6
 * Copyright (C) 2014 Linaro.
 * Viresh Kumar <viresh.kumar@linaro.org>
 *
7 8 9 10 11 12 13 14 15 16 17
 * The OPP code in function cpu0_set_target() is reused from
 * drivers/cpufreq/omap-cpufreq.c
 *
 * This program is free software; you can redistribute it and/or modify
 * it under the terms of the GNU General Public License version 2 as
 * published by the Free Software Foundation.
 */

#define pr_fmt(fmt)	KBUILD_MODNAME ": " fmt

#include <linux/clk.h>
18
#include <linux/cpu.h>
19
#include <linux/cpu_cooling.h>
20
#include <linux/cpufreq.h>
21
#include <linux/cpumask.h>
22 23 24
#include <linux/err.h>
#include <linux/module.h>
#include <linux/of.h>
25
#include <linux/pm_opp.h>
26
#include <linux/platform_device.h>
27 28
#include <linux/regulator/consumer.h>
#include <linux/slab.h>
29
#include <linux/thermal.h>
30

31 32 33 34 35 36
struct private_data {
	struct device *cpu_dev;
	struct regulator *cpu_reg;
	struct thermal_cooling_device *cdev;
	unsigned int voltage_tolerance; /* in percentage */
};
37

38
static int cpu0_set_target(struct cpufreq_policy *policy, unsigned int index)
39
{
40
	struct dev_pm_opp *opp;
41 42 43 44 45
	struct cpufreq_frequency_table *freq_table = policy->freq_table;
	struct clk *cpu_clk = policy->clk;
	struct private_data *priv = policy->driver_data;
	struct device *cpu_dev = priv->cpu_dev;
	struct regulator *cpu_reg = priv->cpu_reg;
46
	unsigned long volt = 0, volt_old = 0, tol = 0;
47
	unsigned int old_freq, new_freq;
48
	long freq_Hz, freq_exact;
49 50 51
	int ret;

	freq_Hz = clk_round_rate(cpu_clk, freq_table[index].frequency * 1000);
52
	if (freq_Hz <= 0)
53 54
		freq_Hz = freq_table[index].frequency * 1000;

55 56 57
	freq_exact = freq_Hz;
	new_freq = freq_Hz / 1000;
	old_freq = clk_get_rate(cpu_clk) / 1000;
58

59
	if (!IS_ERR(cpu_reg)) {
60
		rcu_read_lock();
61
		opp = dev_pm_opp_find_freq_ceil(cpu_dev, &freq_Hz);
62
		if (IS_ERR(opp)) {
63
			rcu_read_unlock();
64 65
			dev_err(cpu_dev, "failed to find OPP for %ld\n",
				freq_Hz);
66
			return PTR_ERR(opp);
67
		}
68
		volt = dev_pm_opp_get_voltage(opp);
69
		rcu_read_unlock();
70
		tol = volt * priv->voltage_tolerance / 100;
71 72 73
		volt_old = regulator_get_voltage(cpu_reg);
	}

74 75 76
	dev_dbg(cpu_dev, "%u MHz, %ld mV --> %u MHz, %ld mV\n",
		old_freq / 1000, volt_old ? volt_old / 1000 : -1,
		new_freq / 1000, volt ? volt / 1000 : -1);
77 78

	/* scaling up?  scale voltage before frequency */
79
	if (!IS_ERR(cpu_reg) && new_freq > old_freq) {
80 81
		ret = regulator_set_voltage_tol(cpu_reg, volt, tol);
		if (ret) {
82 83
			dev_err(cpu_dev, "failed to scale voltage up: %d\n",
				ret);
84
			return ret;
85 86 87
		}
	}

88
	ret = clk_set_rate(cpu_clk, freq_exact);
89
	if (ret) {
90
		dev_err(cpu_dev, "failed to set clock rate: %d\n", ret);
91
		if (!IS_ERR(cpu_reg))
92
			regulator_set_voltage_tol(cpu_reg, volt_old, tol);
93
		return ret;
94 95 96
	}

	/* scaling down?  scale voltage after frequency */
97
	if (!IS_ERR(cpu_reg) && new_freq < old_freq) {
98 99
		ret = regulator_set_voltage_tol(cpu_reg, volt, tol);
		if (ret) {
100 101
			dev_err(cpu_dev, "failed to scale voltage down: %d\n",
				ret);
102
			clk_set_rate(cpu_clk, old_freq * 1000);
103 104 105
		}
	}

106
	return ret;
107 108
}

109 110
static int allocate_resources(struct device **cdev,
			      struct regulator **creg, struct clk **cclk)
111
{
112 113 114 115
	struct device *cpu_dev;
	struct regulator *cpu_reg;
	struct clk *cpu_clk;
	int ret = 0;
116

117 118 119 120 121
	cpu_dev = get_cpu_device(0);
	if (!cpu_dev) {
		pr_err("failed to get cpu0 device\n");
		return -ENODEV;
	}
122

123
	cpu_reg = regulator_get_optional(cpu_dev, "cpu0");
124 125 126 127 128 129
	if (IS_ERR(cpu_reg)) {
		/*
		 * If cpu0 regulator supply node is present, but regulator is
		 * not yet registered, we should try defering probe.
		 */
		if (PTR_ERR(cpu_reg) == -EPROBE_DEFER) {
130
			dev_dbg(cpu_dev, "cpu0 regulator not ready, retry\n");
131
			return -EPROBE_DEFER;
132
		}
133 134
		dev_warn(cpu_dev, "failed to get cpu0 regulator: %ld\n",
			 PTR_ERR(cpu_reg));
135 136
	}

137
	cpu_clk = clk_get(cpu_dev, NULL);
138
	if (IS_ERR(cpu_clk)) {
139 140 141 142
		/* put regulator */
		if (!IS_ERR(cpu_reg))
			regulator_put(cpu_reg);

143
		ret = PTR_ERR(cpu_clk);
144 145 146 147 148 149 150 151 152

		/*
		 * If cpu's clk node is present, but clock is not yet
		 * registered, we should try defering probe.
		 */
		if (ret == -EPROBE_DEFER)
			dev_dbg(cpu_dev, "cpu0 clock not ready, retry\n");
		else
			dev_err(cpu_dev, "failed to get cpu0 clock: %d\n", ret);
153 154 155 156 157 158 159 160 161 162 163 164 165 166 167 168 169 170 171 172 173 174 175 176 177 178 179
	} else {
		*cdev = cpu_dev;
		*creg = cpu_reg;
		*cclk = cpu_clk;
	}

	return ret;
}

static int cpu0_cpufreq_init(struct cpufreq_policy *policy)
{
	struct cpufreq_frequency_table *freq_table;
	struct thermal_cooling_device *cdev;
	struct device_node *np;
	struct private_data *priv;
	struct device *cpu_dev;
	struct regulator *cpu_reg;
	struct clk *cpu_clk;
	unsigned int transition_latency;
	int ret;

	/* We only support cpu0 currently */
	ret = allocate_resources(&cpu_dev, &cpu_reg, &cpu_clk);
	if (ret) {
		pr_err("%s: Failed to allocate resources\n: %d", __func__, ret);
		return ret;
	}
180

181 182 183 184 185
	np = of_node_get(cpu_dev->of_node);
	if (!np) {
		dev_err(cpu_dev, "failed to find cpu%d node\n", policy->cpu);
		ret = -ENOENT;
		goto out_put_reg_clk;
186 187
	}

188 189
	/* OPPs might be populated at runtime, don't check for error here */
	of_init_opp_table(cpu_dev);
190

191
	ret = dev_pm_opp_init_cpufreq_table(cpu_dev, &freq_table);
192
	if (ret) {
193
		dev_err(cpu_dev, "failed to init cpufreq table: %d\n", ret);
194 195 196 197 198 199 200
		goto out_put_node;
	}

	priv = kzalloc(sizeof(*priv), GFP_KERNEL);
	if (!priv) {
		ret = -ENOMEM;
		goto out_free_table;
201 202
	}

203
	of_property_read_u32(np, "voltage-tolerance", &priv->voltage_tolerance);
204 205 206 207

	if (of_property_read_u32(np, "clock-latency", &transition_latency))
		transition_latency = CPUFREQ_ETERNAL;

208
	if (!IS_ERR(cpu_reg)) {
209
		struct dev_pm_opp *opp;
210 211 212 213 214 215 216 217 218 219
		unsigned long min_uV, max_uV;
		int i;

		/*
		 * OPP is maintained in order of increasing frequency, and
		 * freq_table initialised from OPP is therefore sorted in the
		 * same order.
		 */
		for (i = 0; freq_table[i].frequency != CPUFREQ_TABLE_END; i++)
			;
220
		rcu_read_lock();
221
		opp = dev_pm_opp_find_freq_exact(cpu_dev,
222
				freq_table[0].frequency * 1000, true);
223 224
		min_uV = dev_pm_opp_get_voltage(opp);
		opp = dev_pm_opp_find_freq_exact(cpu_dev,
225
				freq_table[i-1].frequency * 1000, true);
226
		max_uV = dev_pm_opp_get_voltage(opp);
227
		rcu_read_unlock();
228 229 230 231 232
		ret = regulator_set_voltage_time(cpu_reg, min_uV, max_uV);
		if (ret > 0)
			transition_latency += ret * 1000;
	}

233 234 235 236 237 238 239
	/*
	 * For now, just loading the cooling device;
	 * thermal DT code takes care of matching them.
	 */
	if (of_find_property(np, "#cooling-cells", NULL)) {
		cdev = of_cpufreq_cooling_register(np, cpu_present_mask);
		if (IS_ERR(cdev))
240 241 242
			dev_err(cpu_dev,
				"running cpufreq without cooling device: %ld\n",
				PTR_ERR(cdev));
243 244
		else
			priv->cdev = cdev;
245
	}
246
	of_node_put(np);
247 248 249 250 251 252 253 254 255 256

	priv->cpu_dev = cpu_dev;
	priv->cpu_reg = cpu_reg;
	policy->driver_data = priv;

	policy->clk = cpu_clk;
	ret = cpufreq_generic_init(policy, freq_table, transition_latency);
	if (ret)
		goto out_cooling_unregister;

257 258
	return 0;

259 260 261
out_cooling_unregister:
	cpufreq_cooling_unregister(priv->cdev);
	kfree(priv);
262
out_free_table:
263
	dev_pm_opp_free_cpufreq_table(cpu_dev, &freq_table);
264 265 266
out_put_node:
	of_node_put(np);
out_put_reg_clk:
267
	clk_put(cpu_clk);
268 269
	if (!IS_ERR(cpu_reg))
		regulator_put(cpu_reg);
270 271 272 273 274 275 276 277 278 279 280 281 282 283 284 285 286 287 288 289 290 291 292 293 294 295 296 297 298 299 300 301 302 303 304 305 306 307 308 309 310 311 312 313 314 315 316 317 318 319 320 321 322 323 324

	return ret;
}

static int cpu0_cpufreq_exit(struct cpufreq_policy *policy)
{
	struct private_data *priv = policy->driver_data;

	cpufreq_cooling_unregister(priv->cdev);
	dev_pm_opp_free_cpufreq_table(priv->cpu_dev, &policy->freq_table);
	clk_put(policy->clk);
	if (!IS_ERR(priv->cpu_reg))
		regulator_put(priv->cpu_reg);
	kfree(priv);

	return 0;
}

static struct cpufreq_driver cpu0_cpufreq_driver = {
	.flags = CPUFREQ_STICKY | CPUFREQ_NEED_INITIAL_FREQ_CHECK,
	.verify = cpufreq_generic_frequency_table_verify,
	.target_index = cpu0_set_target,
	.get = cpufreq_generic_get,
	.init = cpu0_cpufreq_init,
	.exit = cpu0_cpufreq_exit,
	.name = "generic_cpu0",
	.attr = cpufreq_generic_attr,
};

static int cpu0_cpufreq_probe(struct platform_device *pdev)
{
	struct device *cpu_dev;
	struct regulator *cpu_reg;
	struct clk *cpu_clk;
	int ret;

	/*
	 * All per-cluster (CPUs sharing clock/voltages) initialization is done
	 * from ->init(). In probe(), we just need to make sure that clk and
	 * regulators are available. Else defer probe and retry.
	 *
	 * FIXME: Is checking this only for CPU0 sufficient ?
	 */
	ret = allocate_resources(&cpu_dev, &cpu_reg, &cpu_clk);
	if (ret)
		return ret;

	clk_put(cpu_clk);
	if (!IS_ERR(cpu_reg))
		regulator_put(cpu_reg);

	ret = cpufreq_register_driver(&cpu0_cpufreq_driver);
	if (ret)
		dev_err(cpu_dev, "failed register driver: %d\n", ret);

325 326
	return ret;
}
327 328 329 330 331 332 333 334 335 336 337 338 339 340 341 342

static int cpu0_cpufreq_remove(struct platform_device *pdev)
{
	cpufreq_unregister_driver(&cpu0_cpufreq_driver);
	return 0;
}

static struct platform_driver cpu0_cpufreq_platdrv = {
	.driver = {
		.name	= "cpufreq-cpu0",
		.owner	= THIS_MODULE,
	},
	.probe		= cpu0_cpufreq_probe,
	.remove		= cpu0_cpufreq_remove,
};
module_platform_driver(cpu0_cpufreq_platdrv);
343

344
MODULE_AUTHOR("Viresh Kumar <viresh.kumar@linaro.org>");
345 346 347
MODULE_AUTHOR("Shawn Guo <shawn.guo@linaro.org>");
MODULE_DESCRIPTION("Generic CPU0 cpufreq driver");
MODULE_LICENSE("GPL");