From 14bcead6b5ca532f85cffadab2753cc044002f86 Mon Sep 17 00:00:00 2001 From: Christophe Priouzeau Date: Fri, 10 Apr 2020 14:37:04 +0200 Subject: [PATCH 02/23] ARM-stm32mp1-r1-CPUFREQ --- drivers/cpufreq/Kconfig.arm | 7 ++ drivers/cpufreq/Makefile | 1 + drivers/cpufreq/cpufreq-dt-platdev.c | 1 + drivers/cpufreq/stm32-cpufreq.c | 101 +++++++++++++++++++++++++++ drivers/opp/core.c | 11 ++- 5 files changed, 120 insertions(+), 1 deletion(-) create mode 100644 drivers/cpufreq/stm32-cpufreq.c diff --git a/drivers/cpufreq/Kconfig.arm b/drivers/cpufreq/Kconfig.arm index a905796f7..b2beb1708 100644 --- a/drivers/cpufreq/Kconfig.arm +++ b/drivers/cpufreq/Kconfig.arm @@ -295,6 +295,13 @@ config ARM_STI_CPUFREQ this config option if you wish to add CPUFreq support for STi based SoCs. +config ARM_STM32_CPUFREQ + tristate "STM32 CPUFreq support" + depends on MACH_STM32MP157 + default y + help + This adds the CPUFreq driver support for STM32 MPU SOCs. + config ARM_TANGO_CPUFREQ bool depends on CPUFREQ_DT && ARCH_TANGO diff --git a/drivers/cpufreq/Makefile b/drivers/cpufreq/Makefile index 9a9f5ccd1..6139d2aeb 100644 --- a/drivers/cpufreq/Makefile +++ b/drivers/cpufreq/Makefile @@ -80,6 +80,7 @@ obj-$(CONFIG_ARM_SCMI_CPUFREQ) += scmi-cpufreq.o obj-$(CONFIG_ARM_SCPI_CPUFREQ) += scpi-cpufreq.o obj-$(CONFIG_ARM_SPEAR_CPUFREQ) += spear-cpufreq.o obj-$(CONFIG_ARM_STI_CPUFREQ) += sti-cpufreq.o +obj-$(CONFIG_ARM_STM32_CPUFREQ) += stm32-cpufreq.o obj-$(CONFIG_ARM_ALLWINNER_SUN50I_CPUFREQ_NVMEM) += sun50i-cpufreq-nvmem.o obj-$(CONFIG_ARM_TANGO_CPUFREQ) += tango-cpufreq.o obj-$(CONFIG_ARM_TEGRA20_CPUFREQ) += tegra20-cpufreq.o diff --git a/drivers/cpufreq/cpufreq-dt-platdev.c b/drivers/cpufreq/cpufreq-dt-platdev.c index bca8d1f47..7ac5715e4 100644 --- a/drivers/cpufreq/cpufreq-dt-platdev.c +++ b/drivers/cpufreq/cpufreq-dt-platdev.c @@ -131,6 +131,7 @@ static const struct of_device_id blacklist[] __initconst = { { .compatible = "st,stih407", }, { .compatible = "st,stih410", }, + { .compatible = "st,stm32mp157", }, { .compatible = "sigma,tango4", }, diff --git a/drivers/cpufreq/stm32-cpufreq.c b/drivers/cpufreq/stm32-cpufreq.c new file mode 100644 index 000000000..35fb3520d --- /dev/null +++ b/drivers/cpufreq/stm32-cpufreq.c @@ -0,0 +1,101 @@ +// SPDX-License-Identifier: GPL-2.0-only +/* + * Copyright (C) STMicroelectronics 2019 - All Rights Reserved + * Authors: Alexandre Torgue for STMicroelectronics. + */ + +#include +#include +#include +#include +#include +#include +#include +#include + +struct stm32_cpufreq_priv { + struct opp_table *opps; + struct platform_device *cpufreq_dt_pdev; +}; + +static int stm32_cpufreq_probe(struct platform_device *pdev) +{ + struct stm32_cpufreq_priv *priv; + struct device_node *opp_node; + struct device *cpu_dev; + u8 part_number; + u32 supported_hw; + int ret; + + cpu_dev = get_cpu_device(0); + if (!cpu_dev) { + dev_err(&pdev->dev, "failed to get cpu0 device\n"); + return -ENODEV; + } + opp_node = dev_pm_opp_of_get_opp_desc_node(cpu_dev); + if (!opp_node) { + dev_err(&pdev->dev, "OPP-v2 not supported\n"); + return -ENODEV; + } + + /* Get chip info */ + ret = nvmem_cell_read_u8(cpu_dev, "part_number", &part_number); + if (ret) { + dev_err(&pdev->dev, "Failed to get chip info: %d\n", ret); + return ret; + } + + supported_hw = BIT((part_number & 0x80) >> 7); + + priv = devm_kzalloc(&pdev->dev, sizeof(*priv), GFP_KERNEL); + if (!priv) + return -ENOMEM; + + priv->opps = dev_pm_opp_set_supported_hw(cpu_dev, &supported_hw, 1); + if (IS_ERR(priv->opps)) { + ret = PTR_ERR(priv->opps); + if (ret != -EPROBE_DEFER) + dev_err(&pdev->dev, "Failed to set supported opp: %d\n", + ret); + return ret; + } + + of_node_put(opp_node); + priv->cpufreq_dt_pdev = platform_device_register_simple("cpufreq-dt", + -1, NULL, 0); + + platform_set_drvdata(pdev, priv); + + return 0; +} + +static int stm32_cpufreq_remove(struct platform_device *pdev) +{ + struct stm32_cpufreq_priv *priv = platform_get_drvdata(pdev); + + platform_device_unregister(priv->cpufreq_dt_pdev); + dev_pm_opp_put_supported_hw(priv->opps); + + return 0; +} + +static int stm32_cpufreq_init(void) +{ + platform_device_register_simple("stm32-cpufreq", -1, NULL, 0); + + return 0; +} +module_init(stm32_cpufreq_init); + +static struct platform_driver stm32_cpufreq_platdrv = { + .driver = { + .name = "stm32-cpufreq", + }, + .probe = stm32_cpufreq_probe, + .remove = stm32_cpufreq_remove, +}; +module_platform_driver(stm32_cpufreq_platdrv); + +MODULE_DESCRIPTION("STM32 CPU freq driver"); +MODULE_AUTHOR("Alexandre Torgue "); +MODULE_LICENSE("GPL v2"); diff --git a/drivers/opp/core.c b/drivers/opp/core.c index 9ff0538ee..1974bf21f 100644 --- a/drivers/opp/core.c +++ b/drivers/opp/core.c @@ -980,9 +980,14 @@ static struct opp_table *_allocate_opp_table(struct device *dev, int index) opp_table->clk = clk_get(dev, NULL); if (IS_ERR(opp_table->clk)) { ret = PTR_ERR(opp_table->clk); - if (ret != -EPROBE_DEFER) + if (ret != -EPROBE_DEFER) { dev_dbg(dev, "%s: Couldn't find clock: %d\n", __func__, ret); + } + else { + kfree(opp_table); + return ERR_PTR(-EPROBE_DEFER); + } } BLOCKING_INIT_NOTIFIER_HEAD(&opp_table->head); @@ -1443,6 +1448,10 @@ struct opp_table *dev_pm_opp_set_supported_hw(struct device *dev, struct opp_table *opp_table; opp_table = dev_pm_opp_get_opp_table(dev); + + if (PTR_ERR(opp_table) == -EPROBE_DEFER) + return opp_table; + if (!opp_table) return ERR_PTR(-ENOMEM); -- 2.17.1