bmips-cpufreq.c 4.33 KB
Newer Older
1 2 3 4 5 6 7 8 9 10 11 12 13 14 15 16 17 18 19 20 21 22 23 24 25 26 27 28 29 30 31 32 33 34 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 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 98 99 100 101 102 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 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 172 173 174 175 176 177 178 179 180 181 182 183 184 185 186 187 188
/*
 * CPU frequency scaling for Broadcom BMIPS SoCs
 *
 * Copyright (c) 2017 Broadcom
 *
 * This program is free software; you can redistribute it and/or
 * modify it under the terms of the GNU General Public License as
 * published by the Free Software Foundation version 2.
 *
 * This program is distributed "as is" WITHOUT ANY WARRANTY of any
 * kind, whether express or implied; without even the implied warranty
 * of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
 * GNU General Public License for more details.
 */

#include <linux/cpufreq.h>
#include <linux/module.h>
#include <linux/of_address.h>
#include <linux/slab.h>

/* for mips_hpt_frequency */
#include <asm/time.h>

#define BMIPS_CPUFREQ_PREFIX	"bmips"
#define BMIPS_CPUFREQ_NAME	BMIPS_CPUFREQ_PREFIX "-cpufreq"

#define TRANSITION_LATENCY	(25 * 1000)	/* 25 us */

#define BMIPS5_CLK_DIV_SET_SHIFT	0x7
#define BMIPS5_CLK_DIV_SHIFT		0x4
#define BMIPS5_CLK_DIV_MASK		0xf

enum bmips_type {
	BMIPS5000,
	BMIPS5200,
};

struct cpufreq_compat {
	const char *compatible;
	unsigned int bmips_type;
	unsigned int clk_mult;
	unsigned int max_freqs;
};

#define BMIPS(c, t, m, f) { \
	.compatible = c, \
	.bmips_type = (t), \
	.clk_mult = (m), \
	.max_freqs = (f), \
}

static struct cpufreq_compat bmips_cpufreq_compat[] = {
	BMIPS("brcm,bmips5000", BMIPS5000, 8, 4),
	BMIPS("brcm,bmips5200", BMIPS5200, 8, 4),
	{ }
};

static struct cpufreq_compat *priv;

static int htp_freq_to_cpu_freq(unsigned int clk_mult)
{
	return mips_hpt_frequency * clk_mult / 1000;
}

static struct cpufreq_frequency_table *
bmips_cpufreq_get_freq_table(const struct cpufreq_policy *policy)
{
	struct cpufreq_frequency_table *table;
	unsigned long cpu_freq;
	int i;

	cpu_freq = htp_freq_to_cpu_freq(priv->clk_mult);

	table = kmalloc((priv->max_freqs + 1) * sizeof(*table), GFP_KERNEL);
	if (!table)
		return ERR_PTR(-ENOMEM);

	for (i = 0; i < priv->max_freqs; i++) {
		table[i].frequency = cpu_freq / (1 << i);
		table[i].driver_data = i;
	}
	table[i].frequency = CPUFREQ_TABLE_END;

	return table;
}

static unsigned int bmips_cpufreq_get(unsigned int cpu)
{
	unsigned int div;
	uint32_t mode;

	switch (priv->bmips_type) {
	case BMIPS5200:
	case BMIPS5000:
		mode = read_c0_brcm_mode();
		div = ((mode >> BMIPS5_CLK_DIV_SHIFT) & BMIPS5_CLK_DIV_MASK);
		break;
	default:
		div = 0;
	}

	return htp_freq_to_cpu_freq(priv->clk_mult) / (1 << div);
}

static int bmips_cpufreq_target_index(struct cpufreq_policy *policy,
				      unsigned int index)
{
	unsigned int div = policy->freq_table[index].driver_data;

	switch (priv->bmips_type) {
	case BMIPS5200:
	case BMIPS5000:
		change_c0_brcm_mode(BMIPS5_CLK_DIV_MASK << BMIPS5_CLK_DIV_SHIFT,
				    (1 << BMIPS5_CLK_DIV_SET_SHIFT) |
				    (div << BMIPS5_CLK_DIV_SHIFT));
		break;
	default:
		return -ENOTSUPP;
	}

	return 0;
}

static int bmips_cpufreq_exit(struct cpufreq_policy *policy)
{
	kfree(policy->freq_table);

	return 0;
}

static int bmips_cpufreq_init(struct cpufreq_policy *policy)
{
	struct cpufreq_frequency_table *freq_table;
	int ret;

	freq_table = bmips_cpufreq_get_freq_table(policy);
	if (IS_ERR(freq_table)) {
		ret = PTR_ERR(freq_table);
		pr_err("%s: couldn't determine frequency table (%d).\n",
			BMIPS_CPUFREQ_NAME, ret);
		return ret;
	}

	ret = cpufreq_generic_init(policy, freq_table, TRANSITION_LATENCY);
	if (ret)
		bmips_cpufreq_exit(policy);
	else
		pr_info("%s: registered\n", BMIPS_CPUFREQ_NAME);

	return ret;
}

static struct cpufreq_driver bmips_cpufreq_driver = {
	.flags		= CPUFREQ_NEED_INITIAL_FREQ_CHECK,
	.verify		= cpufreq_generic_frequency_table_verify,
	.target_index	= bmips_cpufreq_target_index,
	.get		= bmips_cpufreq_get,
	.init		= bmips_cpufreq_init,
	.exit		= bmips_cpufreq_exit,
	.attr		= cpufreq_generic_attr,
	.name		= BMIPS_CPUFREQ_PREFIX,
};

static int __init bmips_cpufreq_probe(void)
{
	struct cpufreq_compat *cc;
	struct device_node *np;

	for (cc = bmips_cpufreq_compat; cc->compatible; cc++) {
		np = of_find_compatible_node(NULL, "cpu", cc->compatible);
		if (np) {
			of_node_put(np);
			priv = cc;
			break;
		}
	}

	/* We hit the guard element of the array. No compatible CPU found. */
	if (!cc->compatible)
		return -ENODEV;

	return cpufreq_register_driver(&bmips_cpufreq_driver);
}
device_initcall(bmips_cpufreq_probe);

MODULE_AUTHOR("Markus Mayer <mmayer@broadcom.com>");
MODULE_DESCRIPTION("CPUfreq driver for Broadcom BMIPS SoCs");
MODULE_LICENSE("GPL");