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 starfive_cpu_dvfs_info {
  20         struct regulator *vddcpu;
  21         struct clk *cpu_clk;
  22         unsigned long regulator_latency;
  23         struct device *cpu_dev;
  24         struct cpumask cpus;
  25 };
  26
  27 static int starfive_cpufreq_set_target_index(struct cpufreq_policy *policy,
  28                                         unsigned int index)
  29 {
  30         struct cpufreq_frequency_table *freq_table = policy->freq_table;
  31         struct starfive_cpu_dvfs_info *info = cpufreq_get_driver_data();
  32         struct dev_pm_opp *opp;
  33         unsigned long old_freq, new_freq;
  34         int old_vdd, target_vdd, ret;
  35
  36         old_freq = clk_get_rate(info->cpu_clk);
  37         old_vdd = regulator_get_voltage(info->vddcpu);
  38         if (old_vdd < 0) {
  39                 pr_err("Invalid cpu regulator value: %d\n", old_vdd);
  40                 return old_vdd;
  41         }
  42
  43         new_freq = freq_table[index].frequency * 1000;
  44         opp = dev_pm_opp_find_freq_ceil(info->cpu_dev, &new_freq);
  45         if (IS_ERR(opp)) {
  46                 pr_err("Failed to find OPP for %ld\n", new_freq);
  47                 return PTR_ERR(opp);
  48         }
  49         target_vdd = dev_pm_opp_get_voltage(opp);
  50         dev_pm_opp_put(opp);
  51
  52
  53         if (info->vddcpu && new_freq > old_freq) {
  54                 ret = regulator_set_voltage(info->vddcpu,
  55                                            target_vdd, target_vdd + VOLT_TOL);
  56                 if (ret != 0) {
  57                         pr_err("Failed to set vddcpu for %ldkHz: %d\n",
  58                                new_freq, ret);
  59                         return ret;
  60                 }
  61         }
  62
  63         ret = clk_set_rate(info->cpu_clk, new_freq);
  64         if (ret < 0) {
  65                 pr_err("Failed to set rate %ldkHz: %d\n",
  66                        new_freq, ret);
  67         }
  68
  69         if (info->vddcpu && new_freq < old_freq) {
  70                 ret = regulator_set_voltage(info->vddcpu,
  71                                             target_vdd, target_vdd + VOLT_TOL);
  72                 if (ret != 0) {
  73                         pr_err("Failed to set vddcpu for %ldkHz: %d\n",
  74                                new_freq, ret);
  75                         if (clk_set_rate(policy->clk, old_freq * 1000) < 0)
  76                                 pr_err("Failed to restore original clock rate\n");
  77
  78                         return ret;
  79                 }
  80         }
  81
  82         pr_debug("Set actual frequency %lukHz\n",
  83                  clk_get_rate(policy->clk) / 1000);
  84
  85         return 0;
  86 }
  87
  88 static int starfive_cpufreq_driver_init(struct cpufreq_policy *policy)
  89 {
  90         struct starfive_cpu_dvfs_info *info = cpufreq_get_driver_data();
  91         struct cpufreq_frequency_table *freq_table;
  92         int ret;
  93
  94         ret = dev_pm_opp_init_cpufreq_table(info->cpu_dev, &freq_table);
  95         if (ret) {
  96                 pr_err("Failed to init cpufreq table for cpu%d: %d\n",
  97                        policy->cpu, ret);
  98                 return ret;
  99         }
 100
 101         cpumask_copy(policy->cpus, &info->cpus);
 102         policy->freq_table = freq_table;
 103         policy->driver_data = info;
 104         policy->clk = info->cpu_clk;
 105
 106         return 0;
 107 }
 108
 109 static int starfive_cpu_dvfs_info_init(struct platform_device *pdev,
 110                         struct starfive_cpu_dvfs_info *info)
 111 {
 112         struct device *dev = &pdev->dev;
 113         int ret;
 114         static int retry = 3;
 115
 116         info->vddcpu = regulator_get_optional(&pdev->dev, "cpu_vdd_0p9");
 117         if (IS_ERR(info->vddcpu)) {
 118                 if (PTR_ERR(info->vddcpu) == -EPROBE_DEFER)
 119                         dev_warn(&pdev->dev, "The cpu regulator is not ready, retry.\n");
 120                 else
 121                         dev_err(&pdev->dev, "Failed to get regulator for cpu!\n");
 122                 if (retry-- > 0)
 123                         return -EPROBE_DEFER;
 124                 else
 125                         return PTR_ERR(info->vddcpu);
 126         }
 127
 128         info->cpu_clk = devm_clk_get(dev, "cpu_clk");
 129         if (IS_ERR(info->cpu_clk)) {
 130                 dev_err(&pdev->dev, "Unable to obtain cpu_clk: %ld\n",
 131                            PTR_ERR(info->cpu_clk));
 132                 return PTR_ERR(info->cpu_clk);
 133         }
 134
 135         info->cpu_dev = get_cpu_device(1);
 136         if (!info->cpu_dev) {
 137                 dev_err(&pdev->dev, "Failed to get cpu device\n");
 138                 return -ENODEV;
 139         }
 140         /* Get OPP-sharing information from "operating-points-v2" bindings */
 141         ret = dev_pm_opp_of_get_sharing_cpus(info->cpu_dev, &info->cpus);
 142         if (ret) {
 143                 dev_err(&pdev->dev, "Failed to get OPP-sharing information for cpu\n");
 144                 return -EINVAL;
 145         }
 146
 147         ret = dev_pm_opp_of_cpumask_add_table(&info->cpus);
 148         if (ret) {
 149                 pr_warn("no OPP table for cpu\n");
 150                 return -EINVAL;
 151         }
 152
 153         return 0;
 154 }
 155
 156 static struct cpufreq_driver starfive_cpufreq_driver = {
 157         .flags          = CPUFREQ_NEED_INITIAL_FREQ_CHECK,
 158         .verify         = cpufreq_generic_frequency_table_verify,
 159         .target_index   = starfive_cpufreq_set_target_index,
 160         .get            = cpufreq_generic_get,
 161         .init           = starfive_cpufreq_driver_init,
 162         .name           = "starfive-cpufreq",
 163         .attr           = cpufreq_generic_attr,
 164 };
 165
 166 static int starfive_cpufreq_probe(struct platform_device *pdev)
 167 {
 168         struct starfive_cpu_dvfs_info *info;
 169         int ret;
 170
 171         info = devm_kzalloc(&pdev->dev, sizeof(*info), GFP_KERNEL);
 172         if (!info)
 173                 return -ENOMEM;
 174
 175         ret = starfive_cpu_dvfs_info_init(pdev, info);
 176         if (ret) {
 177                 dev_err(&pdev->dev, "Failed to init starfive cpu dvfs info\n");
 178                 return ret;
 179         }
 180
 181         starfive_cpufreq_driver.driver_data = info;
 182         ret = cpufreq_register_driver(&starfive_cpufreq_driver);
 183         if (ret)
 184                 dev_err(&pdev->dev, "Failed to register starfive cpufreq driver\n");
 185
 186         return ret;
 187
 188 }
 189
 190 static const struct of_device_id starfive_cpufreq_match_table[] = {
 191         { .compatible = "starfive,jh7110-cpufreq" },
 192         {}
 193 };
 194
 195 static struct platform_driver starfive_cpufreq_plat_driver = {
 196         .probe = starfive_cpufreq_probe,
 197         .driver = {
 198                 .name = "starfive-cpufreq",
 199                 .of_match_table = starfive_cpufreq_match_table,
 200         },
 201 };
 202
 203 static int __init starfive_cpufreq_init(void)
 204 {
 205         return platform_driver_register(&starfive_cpufreq_plat_driver);
 206 }
 207 device_initcall(starfive_cpufreq_init);
 208
 209 MODULE_DESCRIPTION("STARFIVE CPUFREQ Driver");
 210 MODULE_AUTHOR("Mason Huuo <mason.huo@starfivetech.com>");
 211 MODULE_LICENSE("GPL v2");
 212