qoriq-cpufreq.c 8.39 KB
Newer Older
1 2 3
/*
 * Copyright 2013 Freescale Semiconductor, Inc.
 *
4
 * CPU Frequency Scaling driver for Freescale QorIQ SoCs.
5 6 7 8 9 10 11 12 13 14 15 16 17 18 19 20 21 22 23
 *
 * 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>
#include <linux/cpufreq.h>
#include <linux/errno.h>
#include <linux/init.h>
#include <linux/kernel.h>
#include <linux/module.h>
#include <linux/mutex.h>
#include <linux/of.h>
#include <linux/slab.h>
#include <linux/smp.h>

24
#if !defined(CONFIG_ARM)
25
#include <asm/smp.h>	/* for get_hard_smp_processor_id() in UP configs */
26
#endif
27

28
/**
29
 * struct cpu_data
30
 * @pclk: the parent clock of cpu
31 32 33
 * @table: frequency table
 */
struct cpu_data {
34
	struct clk **pclk;
35 36 37 38 39 40 41 42 43 44 45 46 47 48 49 50 51 52 53 54 55 56 57 58 59 60 61 62 63 64 65 66 67 68 69 70 71 72
	struct cpufreq_frequency_table *table;
};

/**
 * struct soc_data - SoC specific data
 * @freq_mask: mask the disallowed frequencies
 * @flag: unique flags
 */
struct soc_data {
	u32 freq_mask[4];
	u32 flag;
};

#define FREQ_MASK	1
/* see hardware specification for the allowed frqeuencies */
static const struct soc_data sdata[] = {
	{ /* used by p2041 and p3041 */
		.freq_mask = {0x8, 0x8, 0x2, 0x2},
		.flag = FREQ_MASK,
	},
	{ /* used by p5020 */
		.freq_mask = {0x8, 0x2},
		.flag = FREQ_MASK,
	},
	{ /* used by p4080, p5040 */
		.freq_mask = {0},
		.flag = 0,
	},
};

/*
 * the minimum allowed core frequency, in Hz
 * for chassis v1.0, >= platform frequency
 * for chassis v2.0, >= platform frequency / 2
 */
static u32 min_cpufreq;
static const u32 *fmask;

73 74 75 76 77 78 79 80 81 82 83 84 85 86 87 88 89 90 91 92 93 94 95 96 97
#if defined(CONFIG_ARM)
static int get_cpu_physical_id(int cpu)
{
	return topology_core_id(cpu);
}
#else
static int get_cpu_physical_id(int cpu)
{
	return get_hard_smp_processor_id(cpu);
}
#endif

static u32 get_bus_freq(void)
{
	struct device_node *soc;
	u32 sysfreq;

	soc = of_find_node_by_type(NULL, "soc");
	if (!soc)
		return 0;

	if (of_property_read_u32(soc, "bus-frequency", &sysfreq))
		sysfreq = 0;

	of_node_put(soc);
98

99 100
	return sysfreq;
}
101

102
static struct device_node *cpu_to_clk_node(int cpu)
103
{
104 105 106 107 108 109 110 111 112 113 114 115 116 117 118 119 120 121 122 123 124 125 126 127 128 129 130 131 132 133 134 135 136 137 138 139 140 141 142 143
	struct device_node *np, *clk_np;

	if (!cpu_present(cpu))
		return NULL;

	np = of_get_cpu_node(cpu, NULL);
	if (!np)
		return NULL;

	clk_np = of_parse_phandle(np, "clocks", 0);
	if (!clk_np)
		return NULL;

	of_node_put(np);

	return clk_np;
}

/* traverse cpu nodes to get cpu mask of sharing clock wire */
static void set_affected_cpus(struct cpufreq_policy *policy)
{
	struct device_node *np, *clk_np;
	struct cpumask *dstp = policy->cpus;
	int i;

	np = cpu_to_clk_node(policy->cpu);
	if (!np)
		return;

	for_each_present_cpu(i) {
		clk_np = cpu_to_clk_node(i);
		if (!clk_np)
			continue;

		if (clk_np == np)
			cpumask_set_cpu(i, dstp);

		of_node_put(clk_np);
	}
	of_node_put(np);
144 145 146 147 148 149 150 151 152 153 154 155 156 157 158 159 160 161 162 163 164 165 166 167 168 169 170 171
}

/* reduce the duplicated frequencies in frequency table */
static void freq_table_redup(struct cpufreq_frequency_table *freq_table,
		int count)
{
	int i, j;

	for (i = 1; i < count; i++) {
		for (j = 0; j < i; j++) {
			if (freq_table[j].frequency == CPUFREQ_ENTRY_INVALID ||
					freq_table[j].frequency !=
					freq_table[i].frequency)
				continue;

			freq_table[i].frequency = CPUFREQ_ENTRY_INVALID;
			break;
		}
	}
}

/* sort the frequencies in frequency table in descenting order */
static void freq_table_sort(struct cpufreq_frequency_table *freq_table,
		int count)
{
	int i, j, ind;
	unsigned int freq, max_freq;
	struct cpufreq_frequency_table table;
172

173 174 175 176 177 178 179 180 181 182 183 184 185 186 187 188 189 190 191 192 193 194 195 196
	for (i = 0; i < count - 1; i++) {
		max_freq = freq_table[i].frequency;
		ind = i;
		for (j = i + 1; j < count; j++) {
			freq = freq_table[j].frequency;
			if (freq == CPUFREQ_ENTRY_INVALID ||
					freq <= max_freq)
				continue;
			ind = j;
			max_freq = freq;
		}

		if (ind != i) {
			/* exchange the frequencies */
			table.driver_data = freq_table[i].driver_data;
			table.frequency = freq_table[i].frequency;
			freq_table[i].driver_data = freq_table[ind].driver_data;
			freq_table[i].frequency = freq_table[ind].frequency;
			freq_table[ind].driver_data = table.driver_data;
			freq_table[ind].frequency = table.frequency;
		}
	}
}

197
static int qoriq_cpufreq_cpu_init(struct cpufreq_policy *policy)
198
{
199
	struct device_node *np, *pnode;
200 201 202 203 204 205
	int i, count, ret;
	u32 freq, mask;
	struct clk *clk;
	struct cpufreq_frequency_table *table;
	struct cpu_data *data;
	unsigned int cpu = policy->cpu;
206
	u64 u64temp;
207 208 209 210 211 212

	np = of_get_cpu_node(cpu, NULL);
	if (!np)
		return -ENODEV;

	data = kzalloc(sizeof(*data), GFP_KERNEL);
213
	if (!data)
214 215
		goto err_np;

216 217
	policy->clk = of_clk_get(np, 0);
	if (IS_ERR(policy->clk)) {
218 219 220 221
		pr_err("%s: no clock information\n", __func__);
		goto err_nomem2;
	}

222 223
	pnode = of_parse_phandle(np, "clocks", 0);
	if (!pnode) {
224 225 226 227
		pr_err("%s: could not get clock information\n", __func__);
		goto err_nomem2;
	}

228 229 230 231 232 233 234
	count = of_property_count_strings(pnode, "clock-names");
	data->pclk = kcalloc(count, sizeof(struct clk *), GFP_KERNEL);
	if (!data->pclk) {
		pr_err("%s: no memory\n", __func__);
		goto err_node;
	}

235 236 237
	table = kcalloc(count + 1, sizeof(*table), GFP_KERNEL);
	if (!table) {
		pr_err("%s: no memory\n", __func__);
238
		goto err_pclk;
239 240 241
	}

	if (fmask)
242
		mask = fmask[get_cpu_physical_id(cpu)];
243 244 245 246
	else
		mask = 0x0;

	for (i = 0; i < count; i++) {
247 248
		clk = of_clk_get(pnode, i);
		data->pclk[i] = clk;
249 250 251 252 253 254 255 256 257 258 259 260 261 262 263 264
		freq = clk_get_rate(clk);
		/*
		 * the clock is valid if its frequency is not masked
		 * and large than minimum allowed frequency.
		 */
		if (freq < min_cpufreq || (mask & (1 << i)))
			table[i].frequency = CPUFREQ_ENTRY_INVALID;
		else
			table[i].frequency = freq / 1000;
		table[i].driver_data = i;
	}
	freq_table_redup(table, count);
	freq_table_sort(table, count);
	table[i].frequency = CPUFREQ_TABLE_END;

	/* set the min and max frequency properly */
265
	ret = cpufreq_table_validate_and_show(policy, table);
266 267 268 269 270 271 272 273
	if (ret) {
		pr_err("invalid frequency table: %d\n", ret);
		goto err_nomem1;
	}

	data->table = table;

	/* update ->cpus if we have cluster, no harm if not */
274 275
	set_affected_cpus(policy);
	policy->driver_data = data;
276

277 278
	/* Minimum transition latency is 12 platform clocks */
	u64temp = 12ULL * NSEC_PER_SEC;
279
	do_div(u64temp, get_bus_freq());
280
	policy->cpuinfo.transition_latency = u64temp + 1;
281

282
	of_node_put(np);
283
	of_node_put(pnode);
284 285 286 287 288

	return 0;

err_nomem1:
	kfree(table);
289 290
err_pclk:
	kfree(data->pclk);
291
err_node:
292
	of_node_put(pnode);
293
err_nomem2:
294
	policy->driver_data = NULL;
295 296 297 298 299 300 301
	kfree(data);
err_np:
	of_node_put(np);

	return -ENODEV;
}

302
static int __exit qoriq_cpufreq_cpu_exit(struct cpufreq_policy *policy)
303
{
304
	struct cpu_data *data = policy->driver_data;
305

306
	kfree(data->pclk);
307 308
	kfree(data->table);
	kfree(data);
309
	policy->driver_data = NULL;
310 311 312 313

	return 0;
}

314
static int qoriq_cpufreq_target(struct cpufreq_policy *policy,
315
		unsigned int index)
316 317
{
	struct clk *parent;
318
	struct cpu_data *data = policy->driver_data;
319

320
	parent = data->pclk[data->table[index].driver_data];
321
	return clk_set_parent(policy->clk, parent);
322 323
}

324 325
static struct cpufreq_driver qoriq_cpufreq_driver = {
	.name		= "qoriq_cpufreq",
326
	.flags		= CPUFREQ_CONST_LOOPS,
327 328
	.init		= qoriq_cpufreq_cpu_init,
	.exit		= __exit_p(qoriq_cpufreq_cpu_exit),
329
	.verify		= cpufreq_generic_frequency_table_verify,
330
	.target_index	= qoriq_cpufreq_target,
331
	.get		= cpufreq_generic_get,
332
	.attr		= cpufreq_generic_attr,
333 334
};

335
static const struct of_device_id node_matches[] __initconst = {
336 337 338 339 340 341 342 343 344
	{ .compatible = "fsl,p2041-clockgen", .data = &sdata[0], },
	{ .compatible = "fsl,p3041-clockgen", .data = &sdata[0], },
	{ .compatible = "fsl,p5020-clockgen", .data = &sdata[1], },
	{ .compatible = "fsl,p4080-clockgen", .data = &sdata[2], },
	{ .compatible = "fsl,p5040-clockgen", .data = &sdata[2], },
	{ .compatible = "fsl,qoriq-clockgen-2.0", },
	{}
};

345
static int __init qoriq_cpufreq_init(void)
346 347 348 349 350 351 352 353 354 355 356 357 358 359 360
{
	int ret;
	struct device_node  *np;
	const struct of_device_id *match;
	const struct soc_data *data;

	np = of_find_matching_node(NULL, node_matches);
	if (!np)
		return -ENODEV;

	match = of_match_node(node_matches, np);
	data = match->data;
	if (data) {
		if (data->flag)
			fmask = data->freq_mask;
361
		min_cpufreq = get_bus_freq();
362
	} else {
363
		min_cpufreq = get_bus_freq() / 2;
364 365 366 367
	}

	of_node_put(np);

368
	ret = cpufreq_register_driver(&qoriq_cpufreq_driver);
369
	if (!ret)
370
		pr_info("Freescale QorIQ CPU frequency scaling driver\n");
371 372 373

	return ret;
}
374
module_init(qoriq_cpufreq_init);
375

376
static void __exit qoriq_cpufreq_exit(void)
377
{
378
	cpufreq_unregister_driver(&qoriq_cpufreq_driver);
379
}
380
module_exit(qoriq_cpufreq_exit);
381 382 383

MODULE_LICENSE("GPL");
MODULE_AUTHOR("Tang Yuantian <Yuantian.Tang@freescale.com>");
384
MODULE_DESCRIPTION("cpufreq driver for Freescale QorIQ series SoCs");