218 lines · c
1// SPDX-License-Identifier: GPL-2.0+2/*3 * CPUFreq support for Armada 8K4 *5 * Copyright (C) 2018 Marvell6 *7 * Omri Itach <omrii@marvell.com>8 * Gregory Clement <gregory.clement@bootlin.com>9 */10 11#define pr_fmt(fmt) KBUILD_MODNAME ": " fmt12 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 24static 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};29MODULE_DEVICE_TABLE(of, armada_8k_cpufreq_of_match);30 31/*32 * Setup the opps list with the divider for the max frequency, that33 * will be filled at runtime.34 */35static const int opps_div[] __initconst = {1, 2, 3, 4};36 37static struct platform_device *armada_8k_pdev;38 39struct 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. */45static 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 72static 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 102static 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 meant116 * that it was not yet initialized so there is117 * no more opp to free118 */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 130static 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 points153 * supported (which are the nominal CPU frequency and full integer154 * 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 200remove_opp:201 armada_8k_cpufreq_free_table(freq_tables);202 return ret;203}204module_init(armada_8k_cpufreq_init);205 206static 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}213module_exit(armada_8k_cpufreq_exit);214 215MODULE_AUTHOR("Gregory Clement <gregory.clement@bootlin.com>");216MODULE_DESCRIPTION("Armada 8K cpufreq driver");217MODULE_LICENSE("GPL");218