[PATCH 3/3] cpufreq: imx-sc: add i.mx system controller cpufreq support

[Anson Huang <anson.huang@nxp.com>]

On NXP's i.MX SoCs with system controller inside, CPU frequency
scaling can ONLY be done by system controller firmware, and it
can ONLY be requested from secure mode, so Linux cpufreq driver has
to call ARM SMC to trap to ARM-Trusted-Firmware to request system
controller firmware to do CPU frequency scaling.

This patch adds i.MX system controller cpufreq driver support,
when doing CPU frequency scaling, cpufreq driver will do ARM SMC
call and trap to ARM-Trusted-Firmware, then SIP(silicon provider)
service will communicate with system controller for CPU frequenct
scaling.

Signed-off-by: Anson Huang <Anson.Huang@nxp.com>
---
 drivers/cpufreq/Kconfig.arm      |   9 ++
 drivers/cpufreq/Makefile         |   1 +
 drivers/cpufreq/imx-sc-cpufreq.c | 183 +++++++++++++++++++++++++++++++++++++++
 3 files changed, 193 insertions(+)
 create mode 100644 drivers/cpufreq/imx-sc-cpufreq.c

diff --git a/drivers/cpufreq/Kconfig.arm b/drivers/cpufreq/Kconfig.arm
index 179a1d3..6d76465 100644
--- a/drivers/cpufreq/Kconfig.arm
+++ b/drivers/cpufreq/Kconfig.arm
@@ -92,6 +92,15 @@ config ARM_IMX6Q_CPUFREQ
 
 	  If in doubt, say N.
 
+config ARM_IMX_SC_CPUFREQ
+	tristate "NXP i.MX SoC system controller cpufreq support"
+	select PM_OPP
+	help
+	  This adds cpufreq driver support for NXP i.MX series SoCs with system
+	  controller inside.
+
+	  If in doubt, say N.
+
 config ARM_KIRKWOOD_CPUFREQ
 	def_bool MACH_KIRKWOOD
 	help
diff --git a/drivers/cpufreq/Makefile b/drivers/cpufreq/Makefile
index 689b26c..28ab9fb 100644
--- a/drivers/cpufreq/Makefile
+++ b/drivers/cpufreq/Makefile
@@ -56,6 +56,7 @@ obj-$(CONFIG_ACPI_CPPC_CPUFREQ)		+= cppc_cpufreq.o
 obj-$(CONFIG_ARCH_DAVINCI)		+= davinci-cpufreq.o
 obj-$(CONFIG_ARM_HIGHBANK_CPUFREQ)	+= highbank-cpufreq.o
 obj-$(CONFIG_ARM_IMX6Q_CPUFREQ)		+= imx6q-cpufreq.o
+obj-$(CONFIG_ARM_IMX_SC_CPUFREQ)	+= imx-sc-cpufreq.o
 obj-$(CONFIG_ARM_KIRKWOOD_CPUFREQ)	+= kirkwood-cpufreq.o
 obj-$(CONFIG_ARM_MEDIATEK_CPUFREQ)	+= mediatek-cpufreq.o
 obj-$(CONFIG_MACH_MVEBU_V7)		+= mvebu-cpufreq.o
diff --git a/drivers/cpufreq/imx-sc-cpufreq.c b/drivers/cpufreq/imx-sc-cpufreq.c
new file mode 100644
index 0000000..dfc89abe
--- /dev/null
+++ b/drivers/cpufreq/imx-sc-cpufreq.c
@@ -0,0 +1,183 @@
+// SPDX-License-Identifier: GPL-2.0+
+/*
+ * Copyright 2017-2019 NXP.
+ */
+
+#include <linux/arm-smccc.h>
+#include <linux/clk.h>
+#include <linux/cpu.h>
+#include <linux/cpufreq.h>
+#include <linux/err.h>
+#include <linux/firmware/imx/sci.h>
+#include <linux/module.h>
+#include <linux/of.h>
+#include <linux/pm_opp.h>
+#include <linux/platform_device.h>
+#include <linux/syscalls.h>
+
+#define MAX_CLUSTER		1
+
+#define IMX_SIP_CPUFREQ			0xC2000001
+#define IMX_SIP_SET_CPUFREQ		0x00
+
+struct imx_sc_cpufreq {
+	struct device *cpu_dev;
+	struct clk *cpu_clk;
+};
+
+static struct cpufreq_frequency_table *freq_table[MAX_CLUSTER];
+static struct imx_sc_cpufreq cluster_freq[MAX_CLUSTER];
+static unsigned int transition_latency[MAX_CLUSTER];
+static DEFINE_SPINLOCK(cpufreq_psci_lock);
+
+static int imx_sc_cpufreq_set_target(struct cpufreq_policy *policy,
+				     unsigned int index)
+{
+	unsigned int cluster_id = topology_physical_package_id(policy->cpu);
+	unsigned int old_freq, new_freq;
+	struct arm_smccc_res res;
+
+	new_freq = freq_table[cluster_id][index].frequency;
+	old_freq = policy->cur;
+
+	dev_dbg(cluster_freq[cluster_id].cpu_dev, "%u MHz --> %u MHz\n",
+		old_freq / 1000, new_freq / 1000);
+
+	spin_lock(&cpufreq_psci_lock);
+	arm_smccc_smc(IMX_SIP_CPUFREQ, IMX_SIP_SET_CPUFREQ,
+		cluster_id, new_freq * 1000, 0, 0, 0, 0, &res);
+	spin_unlock(&cpufreq_psci_lock);
+
+	return 0;
+}
+
+static int imx_sc_cpufreq_init(struct cpufreq_policy *policy)
+{
+	int cluster_id = topology_physical_package_id(policy->cpu);
+	int opp_num, ret = 0;
+
+	policy->clk = cluster_freq[cluster_id].cpu_clk;
+	policy->cur = clk_get_rate(cluster_freq[cluster_id].cpu_clk) / 1000;
+	cpumask_copy(policy->cpus, topology_core_cpumask(policy->cpu));
+
+	policy->freq_table = freq_table[cluster_id];
+	policy->cpuinfo.transition_latency = transition_latency[cluster_id];
+	opp_num = dev_pm_opp_get_opp_count(cluster_freq[cluster_id].cpu_dev);
+	policy->suspend_freq = freq_table[cluster_id][opp_num - 1].frequency;
+
+	dev_info(cluster_freq[cluster_id].cpu_dev,
+		"cluster %d running at freq %d MHz, suspend freq %d MHz\n",
+		cluster_id, policy->cur / 1000, policy->suspend_freq / 1000);
+
+	return ret;
+}
+
+static struct cpufreq_driver imx_sc_cpufreq_driver = {
+	.flags = CPUFREQ_NEED_INITIAL_FREQ_CHECK |
+		 CPUFREQ_IS_COOLING_DEV,
+	.verify = cpufreq_generic_frequency_table_verify,
+	.target_index = imx_sc_cpufreq_set_target,
+	.get = cpufreq_generic_get,
+	.init = imx_sc_cpufreq_init,
+	.name = "imx-sc-cpufreq",
+	.attr = cpufreq_generic_attr,
+#ifdef CONFIG_PM
+	.suspend = cpufreq_generic_suspend,
+#endif
+};
+
+static int imx_sc_cpufreq_probe(struct platform_device *pdev)
+{
+	struct imx_sc_ipc *cpufreq_ipc_handle;
+	struct device *cpu_dev;
+	struct device_node *np;
+	int cluster_id, ret = 0;
+
+	/* wait mailbox driver ready */
+	ret = imx_scu_get_handle(&cpufreq_ipc_handle);
+	if (ret)
+		return ret;
+
+	cpu_dev = get_cpu_device(0);
+	if (!cpu_dev) {
+		pr_err("failed to get cpu device 0\n");
+		return -ENODEV;
+	}
+
+	np = of_node_get(cpu_dev->of_node);
+	if (!np) {
+		pr_warn("failed to find cpu 0 node\n");
+		return -ENODEV;
+	}
+
+	ret = dev_pm_opp_of_add_table(cpu_dev);
+	if (ret < 0) {
+		dev_err(cpu_dev, "failed to init OPP table: %d\n", ret);
+		goto put_node;
+	}
+
+	cluster_id = topology_physical_package_id(0);
+	cluster_freq[cluster_id].cpu_dev = cpu_dev;
+	cluster_freq[cluster_id].cpu_clk = devm_clk_get(cpu_dev, NULL);
+	if (IS_ERR(cluster_freq[cluster_id].cpu_clk)) {
+		dev_err(cpu_dev, "failed to get cluster %d clock\n",
+			cluster_id);
+		ret = -ENOENT;
+		goto put_node;
+	}
+
+	ret = dev_pm_opp_init_cpufreq_table(cpu_dev, &freq_table[cluster_id]);
+	if (ret) {
+		dev_err(cpu_dev, "failed to init cpufreq table: %d\n", ret);
+		goto out_free_opp;
+	}
+
+	if (of_property_read_u32(np, "clock-latency",
+		&transition_latency[cluster_id]))
+		transition_latency[cluster_id] = CPUFREQ_ETERNAL;
+
+	ret = cpufreq_register_driver(&imx_sc_cpufreq_driver);
+	if (ret) {
+		dev_err(cpu_dev, "failed to register cpufreq driver: %d\n",
+			ret);
+		goto free_freq_table;
+	}
+
+	goto put_node;
+
+free_freq_table:
+	dev_pm_opp_free_cpufreq_table(cpu_dev, &freq_table[cluster_id]);
+out_free_opp:
+	dev_pm_opp_of_remove_table(cpu_dev);
+put_node:
+	of_node_put(np);
+	return ret;
+}
+
+static int imx_sc_cpufreq_remove(struct platform_device *pdev)
+{
+	cpufreq_unregister_driver(&imx_sc_cpufreq_driver);
+
+	return 0;
+}
+
+static int __init imx_sc_register_cpufreq(void)
+{
+	platform_device_register_simple("imx-sc-cpufreq", -1, NULL, 0);
+
+	return 0;
+}
+device_initcall(imx_sc_register_cpufreq);
+
+static struct platform_driver imx_sc_cpufreq_platdrv = {
+	.driver = {
+		.name	= "imx-sc-cpufreq",
+	},
+	.probe		= imx_sc_cpufreq_probe,
+	.remove		= imx_sc_cpufreq_remove,
+};
+module_platform_driver(imx_sc_cpufreq_platdrv);
+
+MODULE_AUTHOR("Anson Huang <Anson.Huang@nxp.com>");
+MODULE_DESCRIPTION("NXP i.MX system controller cpufreq driver");
+MODULE_LICENSE("GPL");
-- 
2.7.4