1 /* 2 * Copyright 2013 Freescale Semiconductor, Inc. 3 * 4 * CPU Frequency Scaling driver for Freescale QorIQ SoCs. 5 * 6 * This program is free software; you can redistribute it and/or modify 7 * it under the terms of the GNU General Public License version 2 as 8 * published by the Free Software Foundation. 9 */ 10 11 #define pr_fmt(fmt) KBUILD_MODNAME ": " fmt 12 13 #include <linux/clk.h> 14 #include <linux/cpufreq.h> 15 #include <linux/cpu_cooling.h> 16 #include <linux/errno.h> 17 #include <linux/init.h> 18 #include <linux/kernel.h> 19 #include <linux/module.h> 20 #include <linux/mutex.h> 21 #include <linux/of.h> 22 #include <linux/slab.h> 23 #include <linux/smp.h> 24 25 #if !defined(CONFIG_ARM) 26 #include <asm/smp.h> /* for get_hard_smp_processor_id() in UP configs */ 27 #endif 28 29 /** 30 * struct cpu_data 31 * @pclk: the parent clock of cpu 32 * @table: frequency table 33 */ 34 struct cpu_data { 35 struct clk **pclk; 36 struct cpufreq_frequency_table *table; 37 struct thermal_cooling_device *cdev; 38 }; 39 40 /** 41 * struct soc_data - SoC specific data 42 * @freq_mask: mask the disallowed frequencies 43 * @flag: unique flags 44 */ 45 struct soc_data { 46 u32 freq_mask[4]; 47 u32 flag; 48 }; 49 50 #define FREQ_MASK 1 51 /* see hardware specification for the allowed frqeuencies */ 52 static const struct soc_data sdata[] = { 53 { /* used by p2041 and p3041 */ 54 .freq_mask = {0x8, 0x8, 0x2, 0x2}, 55 .flag = FREQ_MASK, 56 }, 57 { /* used by p5020 */ 58 .freq_mask = {0x8, 0x2}, 59 .flag = FREQ_MASK, 60 }, 61 { /* used by p4080, p5040 */ 62 .freq_mask = {0}, 63 .flag = 0, 64 }, 65 }; 66 67 /* 68 * the minimum allowed core frequency, in Hz 69 * for chassis v1.0, >= platform frequency 70 * for chassis v2.0, >= platform frequency / 2 71 */ 72 static u32 min_cpufreq; 73 static const u32 *fmask; 74 75 #if defined(CONFIG_ARM) 76 static int get_cpu_physical_id(int cpu) 77 { 78 return topology_core_id(cpu); 79 } 80 #else 81 static int get_cpu_physical_id(int cpu) 82 { 83 return get_hard_smp_processor_id(cpu); 84 } 85 #endif 86 87 static u32 get_bus_freq(void) 88 { 89 struct device_node *soc; 90 u32 sysfreq; 91 92 soc = of_find_node_by_type(NULL, "soc"); 93 if (!soc) 94 return 0; 95 96 if (of_property_read_u32(soc, "bus-frequency", &sysfreq)) 97 sysfreq = 0; 98 99 of_node_put(soc); 100 101 return sysfreq; 102 } 103 104 static struct device_node *cpu_to_clk_node(int cpu) 105 { 106 struct device_node *np, *clk_np; 107 108 if (!cpu_present(cpu)) 109 return NULL; 110 111 np = of_get_cpu_node(cpu, NULL); 112 if (!np) 113 return NULL; 114 115 clk_np = of_parse_phandle(np, "clocks", 0); 116 if (!clk_np) 117 return NULL; 118 119 of_node_put(np); 120 121 return clk_np; 122 } 123 124 /* traverse cpu nodes to get cpu mask of sharing clock wire */ 125 static void set_affected_cpus(struct cpufreq_policy *policy) 126 { 127 struct device_node *np, *clk_np; 128 struct cpumask *dstp = policy->cpus; 129 int i; 130 131 np = cpu_to_clk_node(policy->cpu); 132 if (!np) 133 return; 134 135 for_each_present_cpu(i) { 136 clk_np = cpu_to_clk_node(i); 137 if (!clk_np) 138 continue; 139 140 if (clk_np == np) 141 cpumask_set_cpu(i, dstp); 142 143 of_node_put(clk_np); 144 } 145 of_node_put(np); 146 } 147 148 /* reduce the duplicated frequencies in frequency table */ 149 static void freq_table_redup(struct cpufreq_frequency_table *freq_table, 150 int count) 151 { 152 int i, j; 153 154 for (i = 1; i < count; i++) { 155 for (j = 0; j < i; j++) { 156 if (freq_table[j].frequency == CPUFREQ_ENTRY_INVALID || 157 freq_table[j].frequency != 158 freq_table[i].frequency) 159 continue; 160 161 freq_table[i].frequency = CPUFREQ_ENTRY_INVALID; 162 break; 163 } 164 } 165 } 166 167 /* sort the frequencies in frequency table in descenting order */ 168 static void freq_table_sort(struct cpufreq_frequency_table *freq_table, 169 int count) 170 { 171 int i, j, ind; 172 unsigned int freq, max_freq; 173 struct cpufreq_frequency_table table; 174 175 for (i = 0; i < count - 1; i++) { 176 max_freq = freq_table[i].frequency; 177 ind = i; 178 for (j = i + 1; j < count; j++) { 179 freq = freq_table[j].frequency; 180 if (freq == CPUFREQ_ENTRY_INVALID || 181 freq <= max_freq) 182 continue; 183 ind = j; 184 max_freq = freq; 185 } 186 187 if (ind != i) { 188 /* exchange the frequencies */ 189 table.driver_data = freq_table[i].driver_data; 190 table.frequency = freq_table[i].frequency; 191 freq_table[i].driver_data = freq_table[ind].driver_data; 192 freq_table[i].frequency = freq_table[ind].frequency; 193 freq_table[ind].driver_data = table.driver_data; 194 freq_table[ind].frequency = table.frequency; 195 } 196 } 197 } 198 199 static int qoriq_cpufreq_cpu_init(struct cpufreq_policy *policy) 200 { 201 struct device_node *np, *pnode; 202 int i, count, ret; 203 u32 freq, mask; 204 struct clk *clk; 205 struct cpufreq_frequency_table *table; 206 struct cpu_data *data; 207 unsigned int cpu = policy->cpu; 208 u64 u64temp; 209 210 np = of_get_cpu_node(cpu, NULL); 211 if (!np) 212 return -ENODEV; 213 214 data = kzalloc(sizeof(*data), GFP_KERNEL); 215 if (!data) 216 goto err_np; 217 218 policy->clk = of_clk_get(np, 0); 219 if (IS_ERR(policy->clk)) { 220 pr_err("%s: no clock information\n", __func__); 221 goto err_nomem2; 222 } 223 224 pnode = of_parse_phandle(np, "clocks", 0); 225 if (!pnode) { 226 pr_err("%s: could not get clock information\n", __func__); 227 goto err_nomem2; 228 } 229 230 count = of_property_count_strings(pnode, "clock-names"); 231 data->pclk = kcalloc(count, sizeof(struct clk *), GFP_KERNEL); 232 if (!data->pclk) { 233 pr_err("%s: no memory\n", __func__); 234 goto err_node; 235 } 236 237 table = kcalloc(count + 1, sizeof(*table), GFP_KERNEL); 238 if (!table) { 239 pr_err("%s: no memory\n", __func__); 240 goto err_pclk; 241 } 242 243 if (fmask) 244 mask = fmask[get_cpu_physical_id(cpu)]; 245 else 246 mask = 0x0; 247 248 for (i = 0; i < count; i++) { 249 clk = of_clk_get(pnode, i); 250 data->pclk[i] = clk; 251 freq = clk_get_rate(clk); 252 /* 253 * the clock is valid if its frequency is not masked 254 * and large than minimum allowed frequency. 255 */ 256 if (freq < min_cpufreq || (mask & (1 << i))) 257 table[i].frequency = CPUFREQ_ENTRY_INVALID; 258 else 259 table[i].frequency = freq / 1000; 260 table[i].driver_data = i; 261 } 262 freq_table_redup(table, count); 263 freq_table_sort(table, count); 264 table[i].frequency = CPUFREQ_TABLE_END; 265 266 /* set the min and max frequency properly */ 267 ret = cpufreq_table_validate_and_show(policy, table); 268 if (ret) { 269 pr_err("invalid frequency table: %d\n", ret); 270 goto err_nomem1; 271 } 272 273 data->table = table; 274 275 /* update ->cpus if we have cluster, no harm if not */ 276 set_affected_cpus(policy); 277 policy->driver_data = data; 278 279 /* Minimum transition latency is 12 platform clocks */ 280 u64temp = 12ULL * NSEC_PER_SEC; 281 do_div(u64temp, get_bus_freq()); 282 policy->cpuinfo.transition_latency = u64temp + 1; 283 284 of_node_put(np); 285 of_node_put(pnode); 286 287 return 0; 288 289 err_nomem1: 290 kfree(table); 291 err_pclk: 292 kfree(data->pclk); 293 err_node: 294 of_node_put(pnode); 295 err_nomem2: 296 policy->driver_data = NULL; 297 kfree(data); 298 err_np: 299 of_node_put(np); 300 301 return -ENODEV; 302 } 303 304 static int __exit qoriq_cpufreq_cpu_exit(struct cpufreq_policy *policy) 305 { 306 struct cpu_data *data = policy->driver_data; 307 308 kfree(data->pclk); 309 kfree(data->table); 310 kfree(data); 311 policy->driver_data = NULL; 312 313 return 0; 314 } 315 316 static int qoriq_cpufreq_target(struct cpufreq_policy *policy, 317 unsigned int index) 318 { 319 struct clk *parent; 320 struct cpu_data *data = policy->driver_data; 321 322 parent = data->pclk[data->table[index].driver_data]; 323 return clk_set_parent(policy->clk, parent); 324 } 325 326 327 static void qoriq_cpufreq_ready(struct cpufreq_policy *policy) 328 { 329 struct cpu_data *cpud = policy->driver_data; 330 struct device_node *np = of_get_cpu_node(policy->cpu, NULL); 331 332 if (of_find_property(np, "#cooling-cells", NULL)) { 333 cpud->cdev = of_cpufreq_cooling_register(np, 334 policy->related_cpus); 335 336 if (IS_ERR(cpud->cdev)) { 337 pr_err("Failed to register cooling device cpu%d: %ld\n", 338 policy->cpu, PTR_ERR(cpud->cdev)); 339 340 cpud->cdev = NULL; 341 } 342 } 343 344 of_node_put(np); 345 } 346 347 static struct cpufreq_driver qoriq_cpufreq_driver = { 348 .name = "qoriq_cpufreq", 349 .flags = CPUFREQ_CONST_LOOPS, 350 .init = qoriq_cpufreq_cpu_init, 351 .exit = __exit_p(qoriq_cpufreq_cpu_exit), 352 .verify = cpufreq_generic_frequency_table_verify, 353 .target_index = qoriq_cpufreq_target, 354 .get = cpufreq_generic_get, 355 .ready = qoriq_cpufreq_ready, 356 .attr = cpufreq_generic_attr, 357 }; 358 359 static const struct of_device_id node_matches[] __initconst = { 360 { .compatible = "fsl,p2041-clockgen", .data = &sdata[0], }, 361 { .compatible = "fsl,p3041-clockgen", .data = &sdata[0], }, 362 { .compatible = "fsl,p5020-clockgen", .data = &sdata[1], }, 363 { .compatible = "fsl,p4080-clockgen", .data = &sdata[2], }, 364 { .compatible = "fsl,p5040-clockgen", .data = &sdata[2], }, 365 { .compatible = "fsl,qoriq-clockgen-2.0", }, 366 {} 367 }; 368 369 static int __init qoriq_cpufreq_init(void) 370 { 371 int ret; 372 struct device_node *np; 373 const struct of_device_id *match; 374 const struct soc_data *data; 375 376 np = of_find_matching_node(NULL, node_matches); 377 if (!np) 378 return -ENODEV; 379 380 match = of_match_node(node_matches, np); 381 data = match->data; 382 if (data) { 383 if (data->flag) 384 fmask = data->freq_mask; 385 min_cpufreq = get_bus_freq(); 386 } else { 387 min_cpufreq = get_bus_freq() / 2; 388 } 389 390 of_node_put(np); 391 392 ret = cpufreq_register_driver(&qoriq_cpufreq_driver); 393 if (!ret) 394 pr_info("Freescale QorIQ CPU frequency scaling driver\n"); 395 396 return ret; 397 } 398 module_init(qoriq_cpufreq_init); 399 400 static void __exit qoriq_cpufreq_exit(void) 401 { 402 cpufreq_unregister_driver(&qoriq_cpufreq_driver); 403 } 404 module_exit(qoriq_cpufreq_exit); 405 406 MODULE_LICENSE("GPL"); 407 MODULE_AUTHOR("Tang Yuantian <Yuantian.Tang@freescale.com>"); 408 MODULE_DESCRIPTION("cpufreq driver for Freescale QorIQ series SoCs"); 409