From 5dcf7f3736f15d445a812fec2f6fcc7915b5718f Mon Sep 17 00:00:00 2001 From: "mason.huo" Date: Mon, 20 Jun 2022 11:35:51 +0800 Subject: [PATCH] cpufreq: starfive: Add cpufreq support for Starfive SOC Introduce cpufreq driver which can support CPU frequency adjust in JH7110 soc. Signed-off-by: mason.huo --- drivers/Kconfig | 2 + drivers/cpufreq/Kconfig | 4 + drivers/cpufreq/Kconfig.riscv | 15 +++ drivers/cpufreq/Makefile | 4 + drivers/cpufreq/cpufreq-dt-platdev.c | 1 + drivers/cpufreq/starfive-cpufreq.c | 232 +++++++++++++++++++++++++++++++++++ 6 files changed, 258 insertions(+) create mode 100644 drivers/cpufreq/Kconfig.riscv create mode 100644 drivers/cpufreq/starfive-cpufreq.c diff --git a/drivers/Kconfig b/drivers/Kconfig index f58f714..1898ff9 100644 --- a/drivers/Kconfig +++ b/drivers/Kconfig @@ -241,4 +241,6 @@ source "drivers/e24/Kconfig" source "drivers/xrp/Kconfig" +source "drivers/cpufreq/Kconfig" + endmenu diff --git a/drivers/cpufreq/Kconfig b/drivers/cpufreq/Kconfig index c3038cd..d62547d 100644 --- a/drivers/cpufreq/Kconfig +++ b/drivers/cpufreq/Kconfig @@ -322,4 +322,8 @@ config QORIQ_CPUFREQ which are capable of changing the CPU's frequency dynamically. endif + +if RISCV +source "drivers/cpufreq/Kconfig.riscv" +endif endmenu diff --git a/drivers/cpufreq/Kconfig.riscv b/drivers/cpufreq/Kconfig.riscv new file mode 100644 index 0000000..b375e26 --- /dev/null +++ b/drivers/cpufreq/Kconfig.riscv @@ -0,0 +1,15 @@ +# SPDX-License-Identifier: GPL-2.0-only +# +# RISCV CPU Frequency scaling drivers +# + +config RISCV_STARFIVE_CPUFREQ + bool "Starfive JH7110" + depends on SOC_STARFIVE + default y + help + This adds the CPUFreq driver for Starfive SoC. + + If in doubt, say N. + + diff --git a/drivers/cpufreq/Makefile b/drivers/cpufreq/Makefile index 48ee585..ecbd06c 100644 --- a/drivers/cpufreq/Makefile +++ b/drivers/cpufreq/Makefile @@ -100,6 +100,10 @@ obj-$(CONFIG_PPC_PASEMI_CPUFREQ) += pasemi-cpufreq.o obj-$(CONFIG_POWERNV_CPUFREQ) += powernv-cpufreq.o ################################################################################## +# Riscv platform drivers +obj-$(CONFIG_RISCV_STARFIVE_CPUFREQ) += starfive-cpufreq.o + +################################################################################## # Other platform drivers obj-$(CONFIG_BMIPS_CPUFREQ) += bmips-cpufreq.o obj-$(CONFIG_IA64_ACPI_CPUFREQ) += ia64-acpi-cpufreq.o diff --git a/drivers/cpufreq/cpufreq-dt-platdev.c b/drivers/cpufreq/cpufreq-dt-platdev.c index ca1d103..5cbdeb5 100644 --- a/drivers/cpufreq/cpufreq-dt-platdev.c +++ b/drivers/cpufreq/cpufreq-dt-platdev.c @@ -160,6 +160,7 @@ static const struct of_device_id blocklist[] __initconst = { { .compatible = "qcom,apq8064", }, { .compatible = "qcom,msm8974", }, { .compatible = "qcom,msm8960", }, + { .compatible = "starfive,jh7110", }, { } }; diff --git a/drivers/cpufreq/starfive-cpufreq.c b/drivers/cpufreq/starfive-cpufreq.c new file mode 100644 index 0000000..acfe11c --- /dev/null +++ b/drivers/cpufreq/starfive-cpufreq.c @@ -0,0 +1,232 @@ +// SPDX-License-Identifier: GPL-2.0-only +/* + * Copyright 2022 StarFive Technology Co., Ltd. + * + * Starfive CPUfreq Support + */ + +#include +#include +#include +#include +#include +#include +#include +#include + +#define VOLT_TOL (10000) + +struct stf_cpu_dvfs_info { + struct regulator *vddcpu; + struct clk *cpu_clk; + struct clk *pll0_clk; + struct clk *osc_clk; + unsigned long regulator_latency; + struct device *cpu_dev; + struct cpumask cpus; +}; + +static int stf_cpufreq_set_target_index(struct cpufreq_policy *policy, + unsigned int index) +{ + struct cpufreq_frequency_table *freq_table = policy->freq_table; + struct stf_cpu_dvfs_info *info = cpufreq_get_driver_data(); + struct dev_pm_opp *opp; + unsigned long old_freq, new_freq; + int old_vdd, target_vdd, ret; + + old_freq = clk_get_rate(info->cpu_clk); + old_vdd = regulator_get_voltage(info->vddcpu); + if (old_vdd < 0) { + pr_err("Invalid cpu regulator value: %d\n", old_vdd); + return old_vdd; + } + + new_freq = freq_table[index].frequency * 1000; + opp = dev_pm_opp_find_freq_ceil(info->cpu_dev, &new_freq); + if (IS_ERR(opp)) { + pr_err("Failed to find OPP for %ld\n", new_freq); + return PTR_ERR(opp); + } + target_vdd = dev_pm_opp_get_voltage(opp); + dev_pm_opp_put(opp); + + + if (info->vddcpu && new_freq > old_freq) { + ret = regulator_set_voltage(info->vddcpu, + target_vdd, target_vdd + VOLT_TOL); + if (ret != 0) { + pr_err("Failed to set vddcpu for %ldkHz: %d\n", + new_freq, ret); + return ret; + } + } + + if (clk_set_parent(policy->clk, info->osc_clk)) + pr_err("cpu set parent osc failed\n"); + + ret = clk_set_rate(info->pll0_clk, new_freq); + if (ret < 0) { + pr_err("Failed to set rate %ldkHz: %d\n", + new_freq, ret); + } + if (clk_set_parent(policy->clk, info->pll0_clk)) + pr_err("cpu set parent pll0 failed\n"); + + if (info->vddcpu && new_freq < old_freq) { + ret = regulator_set_voltage(info->vddcpu, + target_vdd, target_vdd + VOLT_TOL); + if (ret != 0) { + pr_err("Failed to set vddcpu for %ldkHz: %d\n", + new_freq, ret); + if (clk_set_rate(policy->clk, old_freq * 1000) < 0) + pr_err("Failed to restore original clock rate\n"); + + return ret; + } + } + + pr_debug("Set actual frequency %lukHz\n", + clk_get_rate(policy->clk) / 1000); + + return 0; +} + +static int stf_cpufreq_driver_init(struct cpufreq_policy *policy) +{ + struct stf_cpu_dvfs_info *info = cpufreq_get_driver_data(); + struct cpufreq_frequency_table *freq_table; + int ret; + + ret = dev_pm_opp_init_cpufreq_table(info->cpu_dev, &freq_table); + if (ret) { + pr_err("Failed to init cpufreq table for cpu%d: %d\n", + policy->cpu, ret); + return ret; + } + + cpumask_copy(policy->cpus, &info->cpus); + policy->freq_table = freq_table; + policy->driver_data = info; + policy->clk = info->cpu_clk; + + return 0; +} + +static int stf_cpu_dvfs_info_init(struct platform_device *pdev, + struct stf_cpu_dvfs_info *info) +{ + struct device *dev = &pdev->dev; + int ret; + static int retry = 3; + + info->vddcpu = regulator_get_optional(&pdev->dev, "cpu_vdd_0p9"); + if (IS_ERR(info->vddcpu)) { + if (PTR_ERR(info->vddcpu) == -EPROBE_DEFER) + dev_warn(&pdev->dev, "The cpu regulator is not ready, retry.\n"); + else + dev_err(&pdev->dev, "Failed to get regulator for cpu\n"); + if (retry-- > 0) + return -EPROBE_DEFER; + else + return PTR_ERR(info->vddcpu); + } + + info->cpu_clk = devm_clk_get(dev, "cpu_clk"); + if (IS_ERR(info->cpu_clk)) { + dev_err(&pdev->dev, "Unable to obtain cpu_clk: %ld\n", + PTR_ERR(info->cpu_clk)); + return PTR_ERR(info->cpu_clk); + } + info->pll0_clk = devm_clk_get(dev, "pll0"); + if (IS_ERR(info->pll0_clk)) { + dev_err(&pdev->dev, "Unable to obtain cpu_clk: %ld\n", + PTR_ERR(info->pll0_clk)); + return PTR_ERR(info->pll0_clk); + } + + info->osc_clk = devm_clk_get(dev, "osc"); + if (IS_ERR(info->osc_clk)) { + dev_err(&pdev->dev, "Unable to obtain osc_clk: %ld\n", + PTR_ERR(info->osc_clk)); + return PTR_ERR(info->osc_clk); + } + + info->cpu_dev = get_cpu_device(1); + if (!info->cpu_dev) { + dev_err(&pdev->dev, "Failed to get cpu device\n"); + return -ENODEV; + } + /* Get OPP-sharing information from "operating-points-v2" bindings */ + ret = dev_pm_opp_of_get_sharing_cpus(info->cpu_dev, &info->cpus); + if (ret) { + dev_err(&pdev->dev, "Failed to get OPP-sharing information for cpu\n"); + return -EINVAL; + } + + ret = dev_pm_opp_of_cpumask_add_table(&info->cpus); + if (ret) { + pr_warn("no OPP table for cpu\n"); + return -EINVAL; + } + + return 0; +} + +static struct cpufreq_driver stf_cpufreq_driver = { + .flags = CPUFREQ_NEED_INITIAL_FREQ_CHECK, + .verify = cpufreq_generic_frequency_table_verify, + .target_index = stf_cpufreq_set_target_index, + .get = cpufreq_generic_get, + .init = stf_cpufreq_driver_init, + .name = "stf-cpufreq", + .attr = cpufreq_generic_attr, +}; + +static int stf_cpufreq_probe(struct platform_device *pdev) +{ + struct stf_cpu_dvfs_info *info; + int ret; + + info = devm_kzalloc(&pdev->dev, sizeof(*info), GFP_KERNEL); + if (!info) + return -ENOMEM; + + ret = stf_cpu_dvfs_info_init(pdev, info); + if (ret) { + dev_err(&pdev->dev, "Failed to init stf cpu dvfs info\n"); + return ret; + } + + stf_cpufreq_driver.driver_data = info; + ret = cpufreq_register_driver(&stf_cpufreq_driver); + if (ret) + dev_err(&pdev->dev, "Failed to register stf cpufreq driver\n"); + + return ret; + +} + +static const struct of_device_id stf_cpufreq_match_table[] = { + { .compatible = "starfive,stf-cpufreq" }, + {} +}; + +static struct platform_driver stf_cpufreq_plat_driver = { + .probe = stf_cpufreq_probe, + .driver = { + .name = "stf-cpufreq", + .of_match_table = stf_cpufreq_match_table, + }, +}; + +static int __init stf_cpufreq_init(void) +{ + return platform_driver_register(&stf_cpufreq_plat_driver); +} +postcore_initcall(stf_cpufreq_init); + +MODULE_DESCRIPTION("STARFIVE CPUFREQ Driver"); +MODULE_AUTHOR("Mason Huuo "); +MODULE_LICENSE("GPL v2"); + -- 2.7.4