armada-8k-cpufreq.c 4.9 KB

123456789101112131415161718192021222324252627282930313233343536373839404142434445464748495051525354555657585960616263646566676869707172737475767778798081828384858687888990919293949596979899100101102103104105106107108109110111112113114115116117118119120121122123124125126127128129130131132133134135136137138139140141142143144145146147148149150151152153154155156157158159160161162163164165166167168169170171172173174175176177178179180181182183184185186187188189190191192193194195196197198199200201202203204205206207208209210211212213214215
  1. // SPDX-License-Identifier: GPL-2.0+
  2. /*
  3. * CPUFreq support for Armada 8K
  4. *
  5. * Copyright (C) 2018 Marvell
  6. *
  7. * Omri Itach <omrii@marvell.com>
  8. * Gregory Clement <gregory.clement@bootlin.com>
  9. */
  10. #define pr_fmt(fmt) KBUILD_MODNAME ": " fmt
  11. #include <linux/clk.h>
  12. #include <linux/cpu.h>
  13. #include <linux/err.h>
  14. #include <linux/init.h>
  15. #include <linux/kernel.h>
  16. #include <linux/module.h>
  17. #include <linux/of.h>
  18. #include <linux/platform_device.h>
  19. #include <linux/pm_opp.h>
  20. #include <linux/slab.h>
  21. /*
  22. * Setup the opps list with the divider for the max frequency, that
  23. * will be filled at runtime.
  24. */
  25. static const int opps_div[] __initconst = {1, 2, 3, 4};
  26. static struct platform_device *armada_8k_pdev;
  27. struct freq_table {
  28. struct device *cpu_dev;
  29. unsigned int freq[ARRAY_SIZE(opps_div)];
  30. };
  31. /* If the CPUs share the same clock, then they are in the same cluster. */
  32. static void __init armada_8k_get_sharing_cpus(struct clk *cur_clk,
  33. struct cpumask *cpumask)
  34. {
  35. int cpu;
  36. for_each_possible_cpu(cpu) {
  37. struct device *cpu_dev;
  38. struct clk *clk;
  39. cpu_dev = get_cpu_device(cpu);
  40. if (!cpu_dev) {
  41. pr_warn("Failed to get cpu%d device\n", cpu);
  42. continue;
  43. }
  44. clk = clk_get(cpu_dev, 0);
  45. if (IS_ERR(clk)) {
  46. pr_warn("Cannot get clock for CPU %d\n", cpu);
  47. } else {
  48. if (clk_is_match(clk, cur_clk))
  49. cpumask_set_cpu(cpu, cpumask);
  50. clk_put(clk);
  51. }
  52. }
  53. }
  54. static int __init armada_8k_add_opp(struct clk *clk, struct device *cpu_dev,
  55. struct freq_table *freq_tables,
  56. int opps_index)
  57. {
  58. unsigned int cur_frequency;
  59. unsigned int freq;
  60. int i, ret;
  61. /* Get nominal (current) CPU frequency. */
  62. cur_frequency = clk_get_rate(clk);
  63. if (!cur_frequency) {
  64. dev_err(cpu_dev, "Failed to get clock rate for this CPU\n");
  65. return -EINVAL;
  66. }
  67. freq_tables[opps_index].cpu_dev = cpu_dev;
  68. for (i = 0; i < ARRAY_SIZE(opps_div); i++) {
  69. freq = cur_frequency / opps_div[i];
  70. ret = dev_pm_opp_add(cpu_dev, freq, 0);
  71. if (ret)
  72. return ret;
  73. freq_tables[opps_index].freq[i] = freq;
  74. }
  75. return 0;
  76. }
  77. static void armada_8k_cpufreq_free_table(struct freq_table *freq_tables)
  78. {
  79. int opps_index, nb_cpus = num_possible_cpus();
  80. for (opps_index = 0 ; opps_index <= nb_cpus; opps_index++) {
  81. int i;
  82. /* If cpu_dev is NULL then we reached the end of the array */
  83. if (!freq_tables[opps_index].cpu_dev)
  84. break;
  85. for (i = 0; i < ARRAY_SIZE(opps_div); i++) {
  86. /*
  87. * A 0Hz frequency is not valid, this meant
  88. * that it was not yet initialized so there is
  89. * no more opp to free
  90. */
  91. if (freq_tables[opps_index].freq[i] == 0)
  92. break;
  93. dev_pm_opp_remove(freq_tables[opps_index].cpu_dev,
  94. freq_tables[opps_index].freq[i]);
  95. }
  96. }
  97. kfree(freq_tables);
  98. }
  99. static int __init armada_8k_cpufreq_init(void)
  100. {
  101. int ret = 0, opps_index = 0, cpu, nb_cpus;
  102. struct freq_table *freq_tables;
  103. struct device_node *node;
  104. struct cpumask cpus;
  105. node = of_find_compatible_node(NULL, NULL, "marvell,ap806-cpu-clock");
  106. if (!node || !of_device_is_available(node)) {
  107. of_node_put(node);
  108. return -ENODEV;
  109. }
  110. of_node_put(node);
  111. nb_cpus = num_possible_cpus();
  112. freq_tables = kcalloc(nb_cpus, sizeof(*freq_tables), GFP_KERNEL);
  113. if (!freq_tables)
  114. return -ENOMEM;
  115. cpumask_copy(&cpus, cpu_possible_mask);
  116. /*
  117. * For each CPU, this loop registers the operating points
  118. * supported (which are the nominal CPU frequency and full integer
  119. * divisions of it).
  120. */
  121. for_each_cpu(cpu, &cpus) {
  122. struct cpumask shared_cpus;
  123. struct device *cpu_dev;
  124. struct clk *clk;
  125. cpu_dev = get_cpu_device(cpu);
  126. if (!cpu_dev) {
  127. pr_err("Cannot get CPU %d\n", cpu);
  128. continue;
  129. }
  130. clk = clk_get(cpu_dev, 0);
  131. if (IS_ERR(clk)) {
  132. pr_err("Cannot get clock for CPU %d\n", cpu);
  133. ret = PTR_ERR(clk);
  134. goto remove_opp;
  135. }
  136. ret = armada_8k_add_opp(clk, cpu_dev, freq_tables, opps_index);
  137. if (ret) {
  138. clk_put(clk);
  139. goto remove_opp;
  140. }
  141. opps_index++;
  142. cpumask_clear(&shared_cpus);
  143. armada_8k_get_sharing_cpus(clk, &shared_cpus);
  144. dev_pm_opp_set_sharing_cpus(cpu_dev, &shared_cpus);
  145. cpumask_andnot(&cpus, &cpus, &shared_cpus);
  146. clk_put(clk);
  147. }
  148. armada_8k_pdev = platform_device_register_simple("cpufreq-dt", -1,
  149. NULL, 0);
  150. ret = PTR_ERR_OR_ZERO(armada_8k_pdev);
  151. if (ret)
  152. goto remove_opp;
  153. platform_set_drvdata(armada_8k_pdev, freq_tables);
  154. return 0;
  155. remove_opp:
  156. armada_8k_cpufreq_free_table(freq_tables);
  157. return ret;
  158. }
  159. module_init(armada_8k_cpufreq_init);
  160. static void __exit armada_8k_cpufreq_exit(void)
  161. {
  162. struct freq_table *freq_tables = platform_get_drvdata(armada_8k_pdev);
  163. platform_device_unregister(armada_8k_pdev);
  164. armada_8k_cpufreq_free_table(freq_tables);
  165. }
  166. module_exit(armada_8k_cpufreq_exit);
  167. static const struct of_device_id __maybe_unused armada_8k_cpufreq_of_match[] = {
  168. { .compatible = "marvell,ap806-cpu-clock" },
  169. { },
  170. };
  171. MODULE_DEVICE_TABLE(of, armada_8k_cpufreq_of_match);
  172. MODULE_AUTHOR("Gregory Clement <gregory.clement@bootlin.com>");
  173. MODULE_DESCRIPTION("Armada 8K cpufreq driver");
  174. MODULE_LICENSE("GPL");