drivers/cpufreq/armada-8k-cpufreq.c

   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");