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 of_node_put(node); 136 137 nb_cpus = num_possible_cpus(); 138 freq_tables = kcalloc(nb_cpus, sizeof(*freq_tables), GFP_KERNEL); 139 if (!freq_tables) 140 return -ENOMEM; 141 cpumask_copy(&cpus, cpu_possible_mask); 142 143 /* 144 * For each CPU, this loop registers the operating points 145 * supported (which are the nominal CPU frequency and full integer 146 * divisions of it). 147 */ 148 for_each_cpu(cpu, &cpus) { 149 struct cpumask shared_cpus; 150 struct device *cpu_dev; 151 struct clk *clk; 152 153 cpu_dev = get_cpu_device(cpu); 154 155 if (!cpu_dev) { 156 pr_err("Cannot get CPU %d\n", cpu); 157 continue; 158 } 159 160 clk = clk_get(cpu_dev, 0); 161 162 if (IS_ERR(clk)) { 163 pr_err("Cannot get clock for CPU %d\n", cpu); 164 ret = PTR_ERR(clk); 165 goto remove_opp; 166 } 167 168 ret = armada_8k_add_opp(clk, cpu_dev, freq_tables, opps_index); 169 if (ret) { 170 clk_put(clk); 171 goto remove_opp; 172 } 173 174 opps_index++; 175 cpumask_clear(&shared_cpus); 176 armada_8k_get_sharing_cpus(clk, &shared_cpus); 177 dev_pm_opp_set_sharing_cpus(cpu_dev, &shared_cpus); 178 cpumask_andnot(&cpus, &cpus, &shared_cpus); 179 clk_put(clk); 180 } 181 182 armada_8k_pdev = platform_device_register_simple("cpufreq-dt", -1, 183 NULL, 0); 184 ret = PTR_ERR_OR_ZERO(armada_8k_pdev); 185 if (ret) 186 goto remove_opp; 187 188 platform_set_drvdata(armada_8k_pdev, freq_tables); 189 190 return 0; 191 192 remove_opp: 193 armada_8k_cpufreq_free_table(freq_tables); 194 return ret; 195 } 196 module_init(armada_8k_cpufreq_init); 197 198 static void __exit armada_8k_cpufreq_exit(void) 199 { 200 struct freq_table *freq_tables = platform_get_drvdata(armada_8k_pdev); 201 202 platform_device_unregister(armada_8k_pdev); 203 armada_8k_cpufreq_free_table(freq_tables); 204 } 205 module_exit(armada_8k_cpufreq_exit); 206 207 static const struct of_device_id __maybe_unused armada_8k_cpufreq_of_match[] = { 208 { .compatible = "marvell,ap806-cpu-clock" }, 209 { }, 210 }; 211 MODULE_DEVICE_TABLE(of, armada_8k_cpufreq_of_match); 212 213 MODULE_AUTHOR("Gregory Clement <gregory.clement@bootlin.com>"); 214 MODULE_DESCRIPTION("Armada 8K cpufreq driver"); 215 MODULE_LICENSE("GPL"); 216