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