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

0001 // SPDX-License-Identifier: GPL-2.0+
0002 /*
0003  * CPUFreq support for Armada 8K
0004  *
0005  * Copyright (C) 2018 Marvell
0006  *
0007  * Omri Itach <omrii@marvell.com>
0008  * Gregory Clement <gregory.clement@bootlin.com>
0009  */
0010
0011 #define pr_fmt(fmt) KBUILD_MODNAME ": " fmt
0012
0013 #include <linux/clk.h>
0014 #include <linux/cpu.h>
0015 #include <linux/err.h>
0016 #include <linux/init.h>
0017 #include <linux/kernel.h>
0018 #include <linux/module.h>
0019 #include <linux/of.h>
0020 #include <linux/platform_device.h>
0021 #include <linux/pm_opp.h>
0022 #include <linux/slab.h>
0023
0024 /*
0025  * Setup the opps list with the divider for the max frequency, that
0026  * will be filled at runtime.
0027  */
0028 static const int opps_div[] __initconst = {1, 2, 3, 4};
0029
0030 static struct platform_device *armada_8k_pdev;
0031
0032 struct freq_table {
0033     struct device *cpu_dev;
0034     unsigned int freq[ARRAY_SIZE(opps_div)];
0035 };
0036
0037 /* If the CPUs share the same clock, then they are in the same cluster. */
0038 static void __init armada_8k_get_sharing_cpus(struct clk *cur_clk,
0039                           struct cpumask *cpumask)
0040 {
0041     int cpu;
0042
0043     for_each_possible_cpu(cpu) {
0044         struct device *cpu_dev;
0045         struct clk *clk;
0046
0047         cpu_dev = get_cpu_device(cpu);
0048         if (!cpu_dev) {
0049             pr_warn("Failed to get cpu%d device\n", cpu);
0050             continue;
0051         }
0052
0053         clk = clk_get(cpu_dev, 0);
0054         if (IS_ERR(clk)) {
0055             pr_warn("Cannot get clock for CPU %d\n", cpu);
0056         } else {
0057             if (clk_is_match(clk, cur_clk))
0058                 cpumask_set_cpu(cpu, cpumask);
0059
0060             clk_put(clk);
0061         }
0062     }
0063 }
0064
0065 static int __init armada_8k_add_opp(struct clk *clk, struct device *cpu_dev,
0066                     struct freq_table *freq_tables,
0067                     int opps_index)
0068 {
0069     unsigned int cur_frequency;
0070     unsigned int freq;
0071     int i, ret;
0072
0073     /* Get nominal (current) CPU frequency. */
0074     cur_frequency = clk_get_rate(clk);
0075     if (!cur_frequency) {
0076         dev_err(cpu_dev, "Failed to get clock rate for this CPU\n");
0077         return -EINVAL;
0078     }
0079
0080     freq_tables[opps_index].cpu_dev = cpu_dev;
0081
0082     for (i = 0; i < ARRAY_SIZE(opps_div); i++) {
0083         freq = cur_frequency / opps_div[i];
0084
0085         ret = dev_pm_opp_add(cpu_dev, freq, 0);
0086         if (ret)
0087             return ret;
0088
0089         freq_tables[opps_index].freq[i] = freq;
0090     }
0091
0092     return 0;
0093 }
0094
0095 static void armada_8k_cpufreq_free_table(struct freq_table *freq_tables)
0096 {
0097     int opps_index, nb_cpus = num_possible_cpus();
0098
0099     for (opps_index = 0 ; opps_index <= nb_cpus; opps_index++) {
0100         int i;
0101
0102         /* If cpu_dev is NULL then we reached the end of the array */
0103         if (!freq_tables[opps_index].cpu_dev)
0104             break;
0105
0106         for (i = 0; i < ARRAY_SIZE(opps_div); i++) {
0107             /*
0108              * A 0Hz frequency is not valid, this meant
0109              * that it was not yet initialized so there is
0110              * no more opp to free
0111              */
0112             if (freq_tables[opps_index].freq[i] == 0)
0113                 break;
0114
0115             dev_pm_opp_remove(freq_tables[opps_index].cpu_dev,
0116                       freq_tables[opps_index].freq[i]);
0117         }
0118     }
0119
0120     kfree(freq_tables);
0121 }
0122
0123 static int __init armada_8k_cpufreq_init(void)
0124 {
0125     int ret = 0, opps_index = 0, cpu, nb_cpus;
0126     struct freq_table *freq_tables;
0127     struct device_node *node;
0128     struct cpumask cpus;
0129
0130     node = of_find_compatible_node(NULL, NULL, "marvell,ap806-cpu-clock");
0131     if (!node || !of_device_is_available(node)) {
0132         of_node_put(node);
0133         return -ENODEV;
0134     }
0135     of_node_put(node);
0136
0137     nb_cpus = num_possible_cpus();
0138     freq_tables = kcalloc(nb_cpus, sizeof(*freq_tables), GFP_KERNEL);
0139     if (!freq_tables)
0140         return -ENOMEM;
0141     cpumask_copy(&cpus, cpu_possible_mask);
0142
0143     /*
0144      * For each CPU, this loop registers the operating points
0145      * supported (which are the nominal CPU frequency and full integer
0146      * divisions of it).
0147      */
0148     for_each_cpu(cpu, &cpus) {
0149         struct cpumask shared_cpus;
0150         struct device *cpu_dev;
0151         struct clk *clk;
0152
0153         cpu_dev = get_cpu_device(cpu);
0154
0155         if (!cpu_dev) {
0156             pr_err("Cannot get CPU %d\n", cpu);
0157             continue;
0158         }
0159
0160         clk = clk_get(cpu_dev, 0);
0161
0162         if (IS_ERR(clk)) {
0163             pr_err("Cannot get clock for CPU %d\n", cpu);
0164             ret = PTR_ERR(clk);
0165             goto remove_opp;
0166         }
0167
0168         ret = armada_8k_add_opp(clk, cpu_dev, freq_tables, opps_index);
0169         if (ret) {
0170             clk_put(clk);
0171             goto remove_opp;
0172         }
0173
0174         opps_index++;
0175         cpumask_clear(&shared_cpus);
0176         armada_8k_get_sharing_cpus(clk, &shared_cpus);
0177         dev_pm_opp_set_sharing_cpus(cpu_dev, &shared_cpus);
0178         cpumask_andnot(&cpus, &cpus, &shared_cpus);
0179         clk_put(clk);
0180     }
0181
0182     armada_8k_pdev = platform_device_register_simple("cpufreq-dt", -1,
0183                              NULL, 0);
0184     ret = PTR_ERR_OR_ZERO(armada_8k_pdev);
0185     if (ret)
0186         goto remove_opp;
0187
0188     platform_set_drvdata(armada_8k_pdev, freq_tables);
0189
0190     return 0;
0191
0192 remove_opp:
0193     armada_8k_cpufreq_free_table(freq_tables);
0194     return ret;
0195 }
0196 module_init(armada_8k_cpufreq_init);
0197
0198 static void __exit armada_8k_cpufreq_exit(void)
0199 {
0200     struct freq_table *freq_tables = platform_get_drvdata(armada_8k_pdev);
0201
0202     platform_device_unregister(armada_8k_pdev);
0203     armada_8k_cpufreq_free_table(freq_tables);
0204 }
0205 module_exit(armada_8k_cpufreq_exit);
0206
0207 static const struct of_device_id __maybe_unused armada_8k_cpufreq_of_match[] = {
0208     { .compatible = "marvell,ap806-cpu-clock" },
0209     { },
0210 };
0211 MODULE_DEVICE_TABLE(of, armada_8k_cpufreq_of_match);
0212
0213 MODULE_AUTHOR("Gregory Clement <gregory.clement@bootlin.com>");
0214 MODULE_DESCRIPTION("Armada 8K cpufreq driver");
0215 MODULE_LICENSE("GPL");