drivers/cpufreq/starfive-cpufreq.c

   1 // SPDX-License-Identifier: GPL-2.0-only
   2 /*
   3  * Copyright 2022 StarFive Technology Co., Ltd.
   4  *
   5  * Starfive CPUfreq Support
   6  */
   7
   8 #include <linux/kernel.h>
   9 #include <linux/types.h>
  10 #include <linux/init.h>
  11 #include <linux/cpufreq.h>
  12 #include <linux/clk.h>
  13 #include <linux/err.h>
  14 #include <linux/regulator/consumer.h>
  15 #include <linux/module.h>
  16
  17 #define VOLT_TOL                (10000)
  18
  19 struct stf_cpu_dvfs_info {
  20         struct regulator *vddcpu;
  21         struct clk *cpu_clk;
  22         struct clk *pll0_clk;
  23         struct clk *osc_clk;
  24         unsigned long regulator_latency;
  25         struct device *cpu_dev;
  26         struct cpumask cpus;
  27 };
  28
  29 static int stf_cpufreq_set_target_index(struct cpufreq_policy *policy,
  30                                         unsigned int index)
  31 {
  32         struct cpufreq_frequency_table *freq_table = policy->freq_table;
  33         struct stf_cpu_dvfs_info *info = cpufreq_get_driver_data();
  34         struct dev_pm_opp *opp;
  35         unsigned long old_freq, new_freq;
  36         int old_vdd, target_vdd, ret;
  37
  38         old_freq = clk_get_rate(info->cpu_clk);
  39         old_vdd = regulator_get_voltage(info->vddcpu);
  40         if (old_vdd < 0) {
  41                 pr_err("Invalid cpu regulator value: %d\n", old_vdd);
  42                 return old_vdd;
  43         }
  44
  45         new_freq = freq_table[index].frequency * 1000;
  46         opp = dev_pm_opp_find_freq_ceil(info->cpu_dev, &new_freq);
  47         if (IS_ERR(opp)) {
  48                 pr_err("Failed to find OPP for %ld\n", new_freq);
  49                 return PTR_ERR(opp);
  50         }
  51         target_vdd = dev_pm_opp_get_voltage(opp);
  52         dev_pm_opp_put(opp);
  53
  54
  55         if (info->vddcpu && new_freq > old_freq) {
  56                 ret = regulator_set_voltage(info->vddcpu,
  57                                            target_vdd, target_vdd + VOLT_TOL);
  58                 if (ret != 0) {
  59                         pr_err("Failed to set vddcpu for %ldkHz: %d\n",
  60                                new_freq, ret);
  61                         return ret;
  62                 }
  63         }
  64
  65         if (clk_set_parent(policy->clk, info->osc_clk))
  66                 pr_err("cpu set parent osc failed\n");
  67
  68         ret = clk_set_rate(info->pll0_clk, new_freq);
  69         if (ret < 0) {
  70                 pr_err("Failed to set rate %ldkHz: %d\n",
  71                        new_freq, ret);
  72         }
  73         if (clk_set_parent(policy->clk, info->pll0_clk))
  74                 pr_err("cpu set parent pll0 failed\n");
  75
  76         if (info->vddcpu && new_freq < old_freq) {
  77                 ret = regulator_set_voltage(info->vddcpu,
  78                                             target_vdd, target_vdd + VOLT_TOL);
  79                 if (ret != 0) {
  80                         pr_err("Failed to set vddcpu for %ldkHz: %d\n",
  81                                new_freq, ret);
  82                         if (clk_set_rate(policy->clk, old_freq * 1000) < 0)
  83                                 pr_err("Failed to restore original clock rate\n");
  84
  85                         return ret;
  86                 }
  87         }
  88
  89         pr_debug("Set actual frequency %lukHz\n",
  90                  clk_get_rate(policy->clk) / 1000);
  91
  92         return 0;
  93 }
  94
  95 static int stf_cpufreq_driver_init(struct cpufreq_policy *policy)
  96 {
  97         struct stf_cpu_dvfs_info *info = cpufreq_get_driver_data();
  98         struct cpufreq_frequency_table *freq_table;
  99         int ret;
 100
 101         ret = dev_pm_opp_init_cpufreq_table(info->cpu_dev, &freq_table);
 102         if (ret) {
 103                 pr_err("Failed to init cpufreq table for cpu%d: %d\n",
 104                        policy->cpu, ret);
 105                 return ret;
 106         }
 107
 108         cpumask_copy(policy->cpus, &info->cpus);
 109         policy->freq_table = freq_table;
 110         policy->driver_data = info;
 111         policy->clk = info->cpu_clk;
 112
 113         return 0;
 114 }
 115
 116 static int stf_cpu_dvfs_info_init(struct platform_device *pdev,
 117                         struct stf_cpu_dvfs_info *info)
 118 {
 119         struct device *dev = &pdev->dev;
 120         int ret;
 121         static int retry = 3;
 122
 123         info->vddcpu = regulator_get_optional(&pdev->dev, "cpu_vdd_0p9");
 124         if (IS_ERR(info->vddcpu)) {
 125                 if (PTR_ERR(info->vddcpu) == -EPROBE_DEFER)
 126                         dev_warn(&pdev->dev, "The cpu regulator is not ready, retry.\n");
 127                 else
 128                         dev_err(&pdev->dev, "Failed to get regulator for cpu\n");
 129                 if (retry-- > 0)
 130                         return -EPROBE_DEFER;
 131                 else
 132                         return PTR_ERR(info->vddcpu);
 133         }
 134
 135         info->cpu_clk = devm_clk_get(dev, "cpu_clk");
 136         if (IS_ERR(info->cpu_clk)) {
 137                 dev_err(&pdev->dev, "Unable to obtain cpu_clk: %ld\n",
 138                            PTR_ERR(info->cpu_clk));
 139                 return PTR_ERR(info->cpu_clk);
 140         }
 141         info->pll0_clk = devm_clk_get(dev, "pll0");
 142         if (IS_ERR(info->pll0_clk)) {
 143                 dev_err(&pdev->dev, "Unable to obtain cpu_clk: %ld\n",
 144                            PTR_ERR(info->pll0_clk));
 145                 return PTR_ERR(info->pll0_clk);
 146         }
 147
 148         info->osc_clk = devm_clk_get(dev, "osc");
 149         if (IS_ERR(info->osc_clk)) {
 150                 dev_err(&pdev->dev, "Unable to obtain osc_clk: %ld\n",
 151                            PTR_ERR(info->osc_clk));
 152                 return PTR_ERR(info->osc_clk);
 153         }
 154
 155         info->cpu_dev = get_cpu_device(1);
 156         if (!info->cpu_dev) {
 157                 dev_err(&pdev->dev, "Failed to get cpu device\n");
 158                 return -ENODEV;
 159         }
 160         /* Get OPP-sharing information from "operating-points-v2" bindings */
 161         ret = dev_pm_opp_of_get_sharing_cpus(info->cpu_dev, &info->cpus);
 162         if (ret) {
 163                 dev_err(&pdev->dev, "Failed to get OPP-sharing information for cpu\n");
 164                 return -EINVAL;
 165         }
 166
 167         ret = dev_pm_opp_of_cpumask_add_table(&info->cpus);
 168         if (ret) {
 169                 pr_warn("no OPP table for cpu\n");
 170                 return -EINVAL;
 171         }
 172
 173         return 0;
 174 }
 175
 176 static struct cpufreq_driver stf_cpufreq_driver = {
 177         .flags          = CPUFREQ_NEED_INITIAL_FREQ_CHECK,
 178         .verify         = cpufreq_generic_frequency_table_verify,
 179         .target_index   = stf_cpufreq_set_target_index,
 180         .get            = cpufreq_generic_get,
 181         .init           = stf_cpufreq_driver_init,
 182         .name           = "stf-cpufreq",
 183         .attr           = cpufreq_generic_attr,
 184 };
 185
 186 static int stf_cpufreq_probe(struct platform_device *pdev)
 187 {
 188         struct stf_cpu_dvfs_info *info;
 189         int ret;
 190
 191         info = devm_kzalloc(&pdev->dev, sizeof(*info), GFP_KERNEL);
 192         if (!info)
 193                 return -ENOMEM;
 194
 195         ret = stf_cpu_dvfs_info_init(pdev, info);
 196         if (ret) {
 197                 dev_err(&pdev->dev, "Failed to init stf cpu dvfs info\n");
 198                 return ret;
 199         }
 200
 201         stf_cpufreq_driver.driver_data = info;
 202         ret = cpufreq_register_driver(&stf_cpufreq_driver);
 203         if (ret)
 204                 dev_err(&pdev->dev, "Failed to register stf cpufreq driver\n");
 205
 206         return ret;
 207
 208 }
 209
 210 static const struct of_device_id stf_cpufreq_match_table[] = {
 211         { .compatible = "starfive,stf-cpufreq" },
 212         {}
 213 };
 214
 215 static struct platform_driver stf_cpufreq_plat_driver = {
 216         .probe = stf_cpufreq_probe,
 217         .driver = {
 218                 .name = "stf-cpufreq",
 219                 .of_match_table = stf_cpufreq_match_table,
 220         },
 221 };
 222
 223 static int __init stf_cpufreq_init(void)
 224 {
 225         return platform_driver_register(&stf_cpufreq_plat_driver);
 226 }
 227 postcore_initcall(stf_cpufreq_init);
 228
 229 MODULE_DESCRIPTION("STARFIVE CPUFREQ Driver");
 230 MODULE_AUTHOR("Mason Huuo <mason.huo@starfivetech.com>");
 231 MODULE_LICENSE("GPL v2");
 232