188 lines · c
1/*2 * CPU frequency scaling for Broadcom BMIPS SoCs3 *4 * Copyright (c) 2017 Broadcom5 *6 * This program is free software; you can redistribute it and/or7 * modify it under the terms of the GNU General Public License as8 * published by the Free Software Foundation version 2.9 *10 * This program is distributed "as is" WITHOUT ANY WARRANTY of any11 * kind, whether express or implied; without even the implied warranty12 * of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the13 * GNU General Public License for more details.14 */15 16#include <linux/cpufreq.h>17#include <linux/module.h>18#include <linux/of_address.h>19#include <linux/slab.h>20 21/* for mips_hpt_frequency */22#include <asm/time.h>23 24#define BMIPS_CPUFREQ_PREFIX "bmips"25#define BMIPS_CPUFREQ_NAME BMIPS_CPUFREQ_PREFIX "-cpufreq"26 27#define TRANSITION_LATENCY (25 * 1000) /* 25 us */28 29#define BMIPS5_CLK_DIV_SET_SHIFT 0x730#define BMIPS5_CLK_DIV_SHIFT 0x431#define BMIPS5_CLK_DIV_MASK 0xf32 33enum bmips_type {34 BMIPS5000,35 BMIPS5200,36};37 38struct cpufreq_compat {39 const char *compatible;40 unsigned int bmips_type;41 unsigned int clk_mult;42 unsigned int max_freqs;43};44 45#define BMIPS(c, t, m, f) { \46 .compatible = c, \47 .bmips_type = (t), \48 .clk_mult = (m), \49 .max_freqs = (f), \50}51 52static struct cpufreq_compat bmips_cpufreq_compat[] = {53 BMIPS("brcm,bmips5000", BMIPS5000, 8, 4),54 BMIPS("brcm,bmips5200", BMIPS5200, 8, 4),55 { }56};57 58static struct cpufreq_compat *priv;59 60static int htp_freq_to_cpu_freq(unsigned int clk_mult)61{62 return mips_hpt_frequency * clk_mult / 1000;63}64 65static struct cpufreq_frequency_table *66bmips_cpufreq_get_freq_table(const struct cpufreq_policy *policy)67{68 struct cpufreq_frequency_table *table;69 unsigned long cpu_freq;70 int i;71 72 cpu_freq = htp_freq_to_cpu_freq(priv->clk_mult);73 74 table = kmalloc_array(priv->max_freqs + 1, sizeof(*table), GFP_KERNEL);75 if (!table)76 return ERR_PTR(-ENOMEM);77 78 for (i = 0; i < priv->max_freqs; i++) {79 table[i].frequency = cpu_freq / (1 << i);80 table[i].driver_data = i;81 }82 table[i].frequency = CPUFREQ_TABLE_END;83 84 return table;85}86 87static unsigned int bmips_cpufreq_get(unsigned int cpu)88{89 unsigned int div;90 uint32_t mode;91 92 switch (priv->bmips_type) {93 case BMIPS5200:94 case BMIPS5000:95 mode = read_c0_brcm_mode();96 div = ((mode >> BMIPS5_CLK_DIV_SHIFT) & BMIPS5_CLK_DIV_MASK);97 break;98 default:99 div = 0;100 }101 102 return htp_freq_to_cpu_freq(priv->clk_mult) / (1 << div);103}104 105static int bmips_cpufreq_target_index(struct cpufreq_policy *policy,106 unsigned int index)107{108 unsigned int div = policy->freq_table[index].driver_data;109 110 switch (priv->bmips_type) {111 case BMIPS5200:112 case BMIPS5000:113 change_c0_brcm_mode(BMIPS5_CLK_DIV_MASK << BMIPS5_CLK_DIV_SHIFT,114 (1 << BMIPS5_CLK_DIV_SET_SHIFT) |115 (div << BMIPS5_CLK_DIV_SHIFT));116 break;117 default:118 return -ENOTSUPP;119 }120 121 return 0;122}123 124static void bmips_cpufreq_exit(struct cpufreq_policy *policy)125{126 kfree(policy->freq_table);127}128 129static int bmips_cpufreq_init(struct cpufreq_policy *policy)130{131 struct cpufreq_frequency_table *freq_table;132 133 freq_table = bmips_cpufreq_get_freq_table(policy);134 if (IS_ERR(freq_table)) {135 pr_err("%s: couldn't determine frequency table (%ld).\n",136 BMIPS_CPUFREQ_NAME, PTR_ERR(freq_table));137 return PTR_ERR(freq_table);138 }139 140 cpufreq_generic_init(policy, freq_table, TRANSITION_LATENCY);141 pr_info("%s: registered\n", BMIPS_CPUFREQ_NAME);142 143 return 0;144}145 146static struct cpufreq_driver bmips_cpufreq_driver = {147 .flags = CPUFREQ_NEED_INITIAL_FREQ_CHECK,148 .verify = cpufreq_generic_frequency_table_verify,149 .target_index = bmips_cpufreq_target_index,150 .get = bmips_cpufreq_get,151 .init = bmips_cpufreq_init,152 .exit = bmips_cpufreq_exit,153 .attr = cpufreq_generic_attr,154 .name = BMIPS_CPUFREQ_PREFIX,155};156 157static int __init bmips_cpufreq_driver_init(void)158{159 struct cpufreq_compat *cc;160 struct device_node *np;161 162 for (cc = bmips_cpufreq_compat; cc->compatible; cc++) {163 np = of_find_compatible_node(NULL, "cpu", cc->compatible);164 if (np) {165 of_node_put(np);166 priv = cc;167 break;168 }169 }170 171 /* We hit the guard element of the array. No compatible CPU found. */172 if (!cc->compatible)173 return -ENODEV;174 175 return cpufreq_register_driver(&bmips_cpufreq_driver);176}177module_init(bmips_cpufreq_driver_init);178 179static void __exit bmips_cpufreq_driver_exit(void)180{181 cpufreq_unregister_driver(&bmips_cpufreq_driver);182}183module_exit(bmips_cpufreq_driver_exit);184 185MODULE_AUTHOR("Markus Mayer <mmayer@broadcom.com>");186MODULE_DESCRIPTION("CPUfreq driver for Broadcom BMIPS SoCs");187MODULE_LICENSE("GPL");188