From dd77db4143290689d3a5e1ec61627233d0711b66 Mon Sep 17 00:00:00 2001
From: Stephen Boyd <sboyd@codeaurora.org>
Date: Fri, 30 May 2014 16:36:11 -0700
Subject: [PATCH] FROMLIST: cpufreq: Add a cpufreq-krait based on cpufreq-cpu0

Krait processors have individual clocks for each CPU that can
scale independently from one another. cpufreq-cpu0 is fairly
close to this, but assumes that there is only one clock for all
CPUs. Add a driver to support the Krait configuration.

TODO: Merge into cpufreq-cpu0? Or make generic?

Signed-off-by: Stephen Boyd <sboyd@codeaurora.org>

---
 drivers/cpufreq/Kconfig         |  13 +++
 drivers/cpufreq/Makefile        |   1 +
 drivers/cpufreq/cpufreq-krait.c | 190 ++++++++++++++++++++++++++++++++++++++++
 3 files changed, 204 insertions(+)
 create mode 100644 drivers/cpufreq/cpufreq-krait.c

--- a/drivers/cpufreq/Kconfig
+++ b/drivers/cpufreq/Kconfig
@@ -198,6 +198,19 @@
 
 	  If in doubt, say N.
 
+config GENERIC_CPUFREQ_KRAIT
+	tristate "Krait cpufreq driver"
+	depends on HAVE_CLK && OF
+	# if CPU_THERMAL is on and THERMAL=m, CPU0 cannot be =y:
+	depends on !CPU_THERMAL || THERMAL
+	select PM_OPP
+	help
+	  This adds a generic cpufreq driver for CPU0 frequency management.
+	  It supports both uniprocessor (UP) and symmetric multiprocessor (SMP)
+	  systems which share clock and voltage across all CPUs.
+
+	  If in doubt, say N.
+
 if X86
 source "drivers/cpufreq/Kconfig.x86"
 endif
--- a/drivers/cpufreq/Makefile
+++ b/drivers/cpufreq/Makefile
@@ -13,6 +13,7 @@
 obj-$(CONFIG_CPU_FREQ_GOV_COMMON)		+= cpufreq_governor.o
 
 obj-$(CONFIG_CPUFREQ_DT)		+= cpufreq-dt.o
+obj-$(CONFIG_GENERIC_CPUFREQ_KRAIT)    += cpufreq-krait.o
 
 ##################################################################################
 # x86 drivers.
--- /dev/null
+++ b/drivers/cpufreq/cpufreq-krait.c
@@ -0,0 +1,390 @@
+/*
+ * Copyright (C) 2012 Freescale Semiconductor, Inc.
+ * Copyright (c) 2014, The Linux Foundation. All rights reserved.
+ *
+ * The OPP code in function krait_set_target() is reused from
+ * drivers/cpufreq/omap-cpufreq.c
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License version 2 as
+ * published by the Free Software Foundation.
+ */
+
+#include <linux/clk.h>
+#include <linux/cpu.h>
+#include <linux/cpu_cooling.h>
+#include <linux/cpufreq.h>
+#include <linux/cpumask.h>
+#include <linux/err.h>
+#include <linux/module.h>
+#include <linux/of.h>
+#include <linux/pm_opp.h>
+#include <linux/platform_device.h>
+#include <linux/regulator/consumer.h>
+#include <linux/slab.h>
+#include <linux/thermal.h>
+
+static unsigned int transition_latency;
+static unsigned int voltage_tolerance; /* in percentage */
+
+static struct device *cpu_dev;
+static DEFINE_PER_CPU(struct clk *, krait_cpu_clks);
+static DEFINE_PER_CPU(struct regulator *, krait_supply_core);
+static struct cpufreq_frequency_table *freq_table;
+static struct thermal_cooling_device *cdev;
+
+struct cache_points {
+	unsigned long cache_freq;
+	unsigned int cache_volt;
+	unsigned long cpu_freq;
+};
+
+static struct regulator *krait_l2_reg;
+static struct clk *krait_l2_clk;
+static struct cache_points *krait_l2_points;
+static int nr_krait_l2_points;
+
+static int krait_parse_cache_points(struct device *dev,
+		struct device_node *of_node)
+{
+	const struct property *prop;
+	const __be32 *val;
+	int nr, i;
+
+	prop = of_find_property(of_node, "cache-points-kHz", NULL);
+	if (!prop)
+		return -ENODEV;
+	if (!prop->value)
+		return -ENODATA;
+
+	/*
+	 * Each OPP is a set of tuples consisting of frequency and
+	 * cpu-frequency like <freq-kHz volt-uV freq-kHz>.
+	 */
+	nr = prop->length / sizeof(u32);
+	if (nr % 3) {
+		dev_err(dev, "%s: Invalid cache points\n", __func__);
+		return -EINVAL;
+	}
+	nr /= 3;
+
+	krait_l2_points = devm_kcalloc(dev, nr, sizeof(*krait_l2_points),
+				       GFP_KERNEL);
+	if (!krait_l2_points)
+		return -ENOMEM;
+	nr_krait_l2_points = nr;
+
+	for (i = 0, val = prop->value; i < nr; i++) {
+		unsigned long cache_freq = be32_to_cpup(val++) * 1000;
+		unsigned int cache_volt = be32_to_cpup(val++);
+		unsigned long cpu_freq = be32_to_cpup(val++) * 1000;
+
+		krait_l2_points[i].cache_freq = cache_freq;
+		krait_l2_points[i].cache_volt = cache_volt;
+		krait_l2_points[i].cpu_freq = cpu_freq;
+	}
+
+	return 0;
+}
+
+static int krait_set_target(struct cpufreq_policy *policy, unsigned int index)
+{
+	struct dev_pm_opp *opp;
+	unsigned long volt = 0, volt_old = 0, tol = 0;
+	unsigned long freq, max_cpu_freq = 0;
+	unsigned int old_freq, new_freq;
+	long freq_Hz, freq_exact;
+	int ret, i;
+	struct clk *cpu_clk;
+	struct regulator *core;
+	unsigned int cpu;
+
+	cpu_clk = per_cpu(krait_cpu_clks, policy->cpu);
+
+	freq_Hz = clk_round_rate(cpu_clk, freq_table[index].frequency * 1000);
+	if (freq_Hz <= 0)
+		freq_Hz = freq_table[index].frequency * 1000;
+
+	freq_exact = freq_Hz;
+	new_freq = freq_Hz / 1000;
+	old_freq = clk_get_rate(cpu_clk) / 1000;
+
+	core = per_cpu(krait_supply_core, policy->cpu);
+
+	rcu_read_lock();
+	opp = dev_pm_opp_find_freq_ceil(cpu_dev, &freq_Hz);
+	if (IS_ERR(opp)) {
+		rcu_read_unlock();
+		pr_err("failed to find OPP for %ld\n", freq_Hz);
+		return PTR_ERR(opp);
+	}
+	volt = dev_pm_opp_get_voltage(opp);
+	rcu_read_unlock();
+	tol = volt * voltage_tolerance / 100;
+	volt_old = regulator_get_voltage(core);
+
+	pr_debug("%u MHz, %ld mV --> %u MHz, %ld mV\n",
+		 old_freq / 1000, volt_old ? volt_old / 1000 : -1,
+		 new_freq / 1000, volt ? volt / 1000 : -1);
+
+	/* scaling up?  scale voltage before frequency */
+	if (new_freq > old_freq) {
+		ret = regulator_set_voltage_tol(core, volt, tol);
+		if (ret) {
+			pr_err("failed to scale voltage up: %d\n", ret);
+			return ret;
+		}
+	}
+
+	ret = clk_set_rate(cpu_clk, freq_exact);
+	if (ret) {
+		pr_err("failed to set clock rate: %d\n", ret);
+		return ret;
+	}
+
+	/* scaling down?  scale voltage after frequency */
+	if (new_freq < old_freq) {
+		ret = regulator_set_voltage_tol(core, volt, tol);
+		if (ret) {
+			pr_err("failed to scale voltage down: %d\n", ret);
+			clk_set_rate(cpu_clk, old_freq * 1000);
+		}
+	}
+
+	for_each_possible_cpu(cpu) {
+		freq = clk_get_rate(per_cpu(krait_cpu_clks, cpu));
+		max_cpu_freq = max(max_cpu_freq, freq);
+	}
+
+	for (i = 0; i < nr_krait_l2_points; i++) {
+		if (max_cpu_freq >= krait_l2_points[i].cpu_freq) {
+			if (krait_l2_reg) {
+				ret = regulator_set_voltage_tol(krait_l2_reg,
+						krait_l2_points[i].cache_volt,
+						tol);
+				if (ret) {
+					pr_err("failed to scale l2 voltage: %d\n",
+						ret);
+				}
+			}
+			ret = clk_set_rate(krait_l2_clk,
+					krait_l2_points[i].cache_freq);
+			if (ret)
+				pr_err("failed to scale l2 clk: %d\n", ret);
+			break;
+		}
+
+	}
+
+	return ret;
+}
+
+static int krait_cpufreq_init(struct cpufreq_policy *policy)
+{
+	int ret;
+
+	policy->clk = per_cpu(krait_cpu_clks, policy->cpu);
+
+	ret = cpufreq_table_validate_and_show(policy, freq_table);
+	if (ret) {
+		pr_err("%s: invalid frequency table: %d\n", __func__, ret);
+		return ret;
+	}
+
+	policy->cpuinfo.transition_latency = transition_latency;
+
+	return 0;
+}
+
+static struct cpufreq_driver krait_cpufreq_driver = {
+	.flags = CPUFREQ_STICKY,
+	.verify = cpufreq_generic_frequency_table_verify,
+	.target_index = krait_set_target,
+	.get = cpufreq_generic_get,
+	.init = krait_cpufreq_init,
+	.name = "generic_krait",
+	.attr = cpufreq_generic_attr,
+};
+
+static int krait_cpufreq_probe(struct platform_device *pdev)
+{
+	struct device_node *np, *cache;
+	int ret, i;
+	unsigned int cpu;
+	struct device *dev;
+	struct clk *clk;
+	struct regulator *core;
+	unsigned long freq_Hz, freq, max_cpu_freq = 0;
+	struct dev_pm_opp *opp;
+	unsigned long volt, tol;
+
+	cpu_dev = get_cpu_device(0);
+	if (!cpu_dev) {
+		pr_err("failed to get krait device\n");
+		return -ENODEV;
+	}
+
+	np = of_node_get(cpu_dev->of_node);
+	if (!np) {
+		pr_err("failed to find krait node\n");
+		return -ENOENT;
+	}
+
+	ret = dev_pm_opp_init_cpufreq_table(cpu_dev, &freq_table);
+	if (ret) {
+		pr_err("failed to init cpufreq table: %d\n", ret);
+		goto out_put_node;
+	}
+
+	of_property_read_u32(np, "voltage-tolerance", &voltage_tolerance);
+
+	if (of_property_read_u32(np, "clock-latency", &transition_latency))
+		transition_latency = CPUFREQ_ETERNAL;
+
+	cache = of_find_next_cache_node(np);
+	if (cache) {
+		struct device_node *vdd;
+
+		vdd = of_parse_phandle(cache, "vdd_dig-supply", 0);
+		if (vdd) {
+			krait_l2_reg = regulator_get(NULL, vdd->name);
+			if (IS_ERR(krait_l2_reg)) {
+				pr_warn("failed to get l2 vdd_dig supply\n");
+				krait_l2_reg = NULL;
+			}
+			of_node_put(vdd);
+		}
+
+		krait_l2_clk = of_clk_get(cache, 0);
+		if (!IS_ERR(krait_l2_clk)) {
+			ret = krait_parse_cache_points(&pdev->dev, cache);
+			if (ret)
+				clk_put(krait_l2_clk);
+		}
+		if (IS_ERR(krait_l2_clk) || ret)
+			krait_l2_clk = NULL;
+	}
+
+	for_each_possible_cpu(cpu) {
+		dev = get_cpu_device(cpu);
+		if (!dev) {
+			pr_err("failed to get krait device\n");
+			ret = -ENOENT;
+			goto out_free_table;
+		}
+		per_cpu(krait_cpu_clks, cpu) = clk = devm_clk_get(dev, NULL);
+		if (IS_ERR(clk)) {
+			ret = PTR_ERR(clk);
+			goto out_free_table;
+		}
+		core = devm_regulator_get(dev, "core");
+		if (IS_ERR(core)) {
+			pr_debug("failed to get core regulator\n");
+			ret = PTR_ERR(core);
+			goto out_free_table;
+		}
+		per_cpu(krait_supply_core, cpu) = core;
+
+		freq = freq_Hz = clk_get_rate(clk);
+
+		rcu_read_lock();
+		opp = dev_pm_opp_find_freq_ceil(cpu_dev, &freq_Hz);
+		if (IS_ERR(opp)) {
+			rcu_read_unlock();
+			pr_err("failed to find OPP for %ld\n", freq_Hz);
+			ret = PTR_ERR(opp);
+			goto out_free_table;
+		}
+		volt = dev_pm_opp_get_voltage(opp);
+		rcu_read_unlock();
+
+		tol = volt * voltage_tolerance / 100;
+		ret = regulator_set_voltage_tol(core, volt, tol);
+		if (ret) {
+			pr_err("failed to scale voltage up: %d\n", ret);
+			goto out_free_table;
+		}
+		ret = regulator_enable(core);
+		if (ret) {
+			pr_err("failed to enable regulator: %d\n", ret);
+			goto out_free_table;
+		}
+		max_cpu_freq = max(max_cpu_freq, freq);
+	}
+
+	for (i = 0; i < nr_krait_l2_points; i++) {
+		if (max_cpu_freq >= krait_l2_points[i].cpu_freq) {
+			if (krait_l2_reg) {
+				ret = regulator_set_voltage_tol(krait_l2_reg,
+						krait_l2_points[i].cache_volt,
+						tol);
+				if (ret)
+					pr_err("failed to scale l2 voltage: %d\n",
+							ret);
+				ret = regulator_enable(krait_l2_reg);
+				if (ret)
+					pr_err("failed to enable l2 voltage: %d\n",
+							ret);
+			}
+			break;
+		}
+
+	}
+
+	ret = cpufreq_register_driver(&krait_cpufreq_driver);
+	if (ret) {
+		pr_err("failed register driver: %d\n", ret);
+		goto out_free_table;
+	}
+	of_node_put(np);
+
+	/*
+	 * For now, just loading the cooling device;
+	 * thermal DT code takes care of matching them.
+	 */
+	for_each_possible_cpu(cpu) {
+		dev = get_cpu_device(cpu);
+		np = of_node_get(dev->of_node);
+		if (of_find_property(np, "#cooling-cells", NULL)) {
+			cdev = of_cpufreq_cooling_register(np, cpumask_of(cpu));
+			if (IS_ERR(cdev))
+				pr_err("running cpufreq without cooling device: %ld\n",
+				       PTR_ERR(cdev));
+		}
+		of_node_put(np);
+	}
+
+	return 0;
+
+out_free_table:
+	regulator_put(krait_l2_reg);
+	clk_put(krait_l2_clk);
+	dev_pm_opp_free_cpufreq_table(cpu_dev, &freq_table);
+out_put_node:
+	of_node_put(np);
+	return ret;
+}
+
+static int krait_cpufreq_remove(struct platform_device *pdev)
+{
+	cpufreq_cooling_unregister(cdev);
+	cpufreq_unregister_driver(&krait_cpufreq_driver);
+	dev_pm_opp_free_cpufreq_table(cpu_dev, &freq_table);
+	clk_put(krait_l2_clk);
+	regulator_put(krait_l2_reg);
+
+	return 0;
+}
+
+static struct platform_driver krait_cpufreq_platdrv = {
+	.driver = {
+		.name	= "cpufreq-krait",
+		.owner	= THIS_MODULE,
+	},
+	.probe		= krait_cpufreq_probe,
+	.remove		= krait_cpufreq_remove,
+};
+module_platform_driver(krait_cpufreq_platdrv);
+
+MODULE_DESCRIPTION("Krait CPUfreq driver");
+MODULE_LICENSE("GPL v2");
--- a/drivers/cpufreq/qcom-cpufreq.c
+++ b/drivers/cpufreq/qcom-cpufreq.c
@@ -168,11 +168,8 @@
 
 static int __init qcom_cpufreq_driver_init(void)
 {
-	struct cpufreq_dt_platform_data pdata = { .independent_clocks = true };
 	struct platform_device_info devinfo = {
-		.name = "cpufreq-dt",
-		.data = &pdata,
-		.size_data = sizeof(pdata),
+		.name = "cpufreq-krait",
 	};
 	struct device *cpu_dev;
 	struct device_node *np;