From 704a13efcc696fa4d151b741e0882f4c42eef870 Mon Sep 17 00:00:00 2001
From: Romuald JEANNE <romuald.jeanne@st.com>
Date: Thu, 30 Jan 2020 15:28:05 +0100
Subject: [PATCH 02/31] ARM stm32mp1 r3 CPUFREQ

---
 drivers/cpufreq/Kconfig.arm          |  7 +++
 drivers/cpufreq/Makefile             |  1 +
 drivers/cpufreq/cpufreq-dt-platdev.c |  1 +
 drivers/cpufreq/stm32-cpufreq.c      | 99 ++++++++++++++++++++++++++++++++++++
 4 files changed, 108 insertions(+)
 create mode 100644 drivers/cpufreq/stm32-cpufreq.c

diff --git a/drivers/cpufreq/Kconfig.arm b/drivers/cpufreq/Kconfig.arm
index 0cd8eb7..b950d5a 100644
--- a/drivers/cpufreq/Kconfig.arm
+++ b/drivers/cpufreq/Kconfig.arm
@@ -255,6 +255,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 c1ffeab..c15b4f7 100644
--- a/drivers/cpufreq/Makefile
+++ b/drivers/cpufreq/Makefile
@@ -79,6 +79,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_TANGO_CPUFREQ)		+= tango-cpufreq.o
 obj-$(CONFIG_ARM_TEGRA20_CPUFREQ)	+= tegra20-cpufreq.o
 obj-$(CONFIG_ARM_TEGRA124_CPUFREQ)	+= tegra124-cpufreq.o
diff --git a/drivers/cpufreq/cpufreq-dt-platdev.c b/drivers/cpufreq/cpufreq-dt-platdev.c
index fe14c57..d61a02a 100644
--- a/drivers/cpufreq/cpufreq-dt-platdev.c
+++ b/drivers/cpufreq/cpufreq-dt-platdev.c
@@ -121,6 +121,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 0000000..f4a41e0
--- /dev/null
+++ b/drivers/cpufreq/stm32-cpufreq.c
@@ -0,0 +1,99 @@
+// SPDX-License-Identifier: GPL-2.0-only
+/*
+ * Copyright (C) STMicroelectronics 2019 - All Rights Reserved
+ * Authors: Alexandre Torgue <alexandre.torgue@st.com> for STMicroelectronics.
+ */
+
+#include <linux/cpu.h>
+#include <linux/cpufreq.h>
+#include <linux/init.h>
+#include <linux/kernel.h>
+#include <linux/module.h>
+#include <linux/nvmem-consumer.h>
+#include <linux/of_platform.h>
+#include <linux/pm_opp.h>
+
+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);
+		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 <alexandre.torgue@st.com>");
+MODULE_LICENSE("GPL v2");
-- 
2.7.4