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