drivers: thermal: sdpm: Add SDPM clock notifier driver

Add SDPM clock notifier driver, which will register for clock rate
change notification and write the clock rate into SDPM CSR register. The
SDPM hardware will monitor the frequency changes and recommend
mitigation using policy engine.

Change-Id: I56b2941b3c3e47bdb49bf805a2fc9acd4356470f
Signed-off-by: Ram Chandrasekar <rkumbako@codeaurora.org>

drivers/thermal/qcom/Kconfig

@@ -109,3 +109,12 @@ config QTI_POLICY_ENGINE_SENSOR
 	    register the policy engine recommendation with thermal framework as
 	    a sensor. This will enable to provide configuration to mitigate
 	    cooling devices when a recommendation is sent from hardware.
+
+config QTI_SDPM_CLOCK_MONITOR
+	tristate "QTI SDPM Clock Monitor"
+	depends on COMMON_CLK && CPU_FREQ
+	help
+	    This enables the QTI SDPM Clock Monitor. This driver can register
+	    for different clock rate change notifications and write the clock
+	    rate into the SDPM CSR register. This driver will receive the clock
+	    list and the CSR details from devicetree.

drivers/thermal/qcom/Makefile

@@ -18,3 +18,4 @@ obj-$(CONFIG_QTI_BCL_SOC_DRIVER) += bcl_soc.o
 obj-$(CONFIG_QTI_THERMAL_LIMITS_DCVS) += msm_lmh_dcvs.o
 obj-$(CONFIG_QTI_CPU_VOLTAGE_COOLING_DEVICE) += cpu_voltage_cooling.o
 obj-$(CONFIG_QTI_POLICY_ENGINE_SENSOR) += policy_engine.o
+obj-${CONFIG_QTI_SDPM_CLOCK_MONITOR} += sdpm_clk.o

drivers/thermal/qcom/sdpm_clk.c

@@ -0,0 +1,284 @@
+// SPDX-License-Identifier: GPL-2.0-only
+/*
+ * Copyright (c) 2020, The Linux Foundation. All rights reserved.
+ */
+
+#include <linux/module.h>
+#include <linux/clk.h>
+#include <linux/platform_device.h>
+#include <linux/err.h>
+#include <linux/slab.h>
+#include <linux/of.h>
+#include <linux/of_address.h>
+#include <linux/cpufreq.h>
+#include <linux/pm_opp.h>
+#include <linux/cpu.h>
+
+#define SDPM_DRIVER		"sdpm-clk-notify"
+#define CSR_MAX_VAL		7
+#define CSR_OFFSET		0xF00
+#define FREQ_HZ_TO_MHZ(f)	((f) / 1000000)
+#define FREQ_KHZ_TO_MHZ(f)	((f) / 1000)
+
+struct sdpm_clk_instance;
+struct sdpm_clk_data {
+	struct notifier_block		clk_rate_nb;
+	struct clk			*clk;
+	const char			*clock_name;
+	uint32_t			cpu_id;
+	uint32_t			csr_id;
+	unsigned long			last_freq;
+	struct sdpm_clk_instance	*sdpm_inst;
+};
+struct sdpm_clk_instance {
+	struct device			*dev;
+	void __iomem			*regmap;
+	uint32_t			clk_ct;
+	struct sdpm_clk_data		*clk_data;
+};
+
+static void sdpm_csr_write(struct sdpm_clk_data *sdpm_data,
+				unsigned long clk_rate)
+{
+	struct sdpm_clk_instance *sdpm_inst = sdpm_data->sdpm_inst;
+	uint32_t val = clk_rate;
+
+	sdpm_data->last_freq = clk_rate;
+
+	dev_dbg(sdpm_inst->dev, "clock:%s offset:0x%x frequency:%u\n",
+			(sdpm_data->clock_name) ? sdpm_data->clock_name :
+			"cpu", CSR_OFFSET + sdpm_data->csr_id * 4,
+			val);
+	writel_relaxed(val,
+		sdpm_inst->regmap + CSR_OFFSET + sdpm_data->csr_id * 4);
+}
+
+static int sdpm_cpu_notifier(struct notifier_block *nb,
+					unsigned long event, void *data)
+{
+	struct cpufreq_freqs *freq_data = (struct cpufreq_freqs *)data;
+	struct sdpm_clk_data *sdpm_data = container_of(nb,
+				struct sdpm_clk_data, clk_rate_nb);
+
+	if (freq_data->policy->cpu != sdpm_data->cpu_id)
+		return NOTIFY_DONE;
+	dev_dbg(sdpm_data->sdpm_inst->dev, "CPU%d event:%lu\n",
+			freq_data->policy->cpu, event);
+	switch (event) {
+	case CPUFREQ_PRECHANGE:
+		if (freq_data->new > freq_data->old)
+			sdpm_csr_write(sdpm_data,
+					FREQ_KHZ_TO_MHZ(freq_data->new));
+		return NOTIFY_DONE;
+	case CPUFREQ_POSTCHANGE:
+		if (freq_data->new < freq_data->old)
+			sdpm_csr_write(sdpm_data,
+					FREQ_KHZ_TO_MHZ(freq_data->new));
+		return NOTIFY_DONE;
+	default:
+		return NOTIFY_DONE;
+	}
+	return NOTIFY_DONE;
+}
+
+static int sdpm_clock_notifier(struct notifier_block *nb,
+					unsigned long event, void *data)
+{
+	struct clk_notifier_data *ndata = data;
+	struct sdpm_clk_data *sdpm_data = container_of(nb,
+				struct sdpm_clk_data, clk_rate_nb);
+
+	dev_dbg(sdpm_data->sdpm_inst->dev, "clock:%s event:%lu\n",
+			sdpm_data->clock_name, event);
+	switch (event) {
+	case PRE_RATE_CHANGE:
+		if (ndata->new_rate > ndata->old_rate)
+			sdpm_csr_write(sdpm_data,
+					FREQ_HZ_TO_MHZ(ndata->new_rate));
+		return NOTIFY_DONE;
+	case POST_RATE_CHANGE:
+		if (ndata->new_rate < ndata->old_rate)
+			sdpm_csr_write(sdpm_data,
+					FREQ_HZ_TO_MHZ(ndata->new_rate));
+		return NOTIFY_DONE;
+	case ABORT_RATE_CHANGE:
+		if (ndata->new_rate > ndata->old_rate)
+			sdpm_csr_write(sdpm_data,
+					FREQ_HZ_TO_MHZ(ndata->old_rate));
+		return NOTIFY_DONE;
+	default:
+		return NOTIFY_DONE;
+	}
+}
+
+static int sdpm_clk_device_probe(struct platform_device *pdev)
+{
+	struct device *dev = &pdev->dev;
+	int ret = 0, idx = 0, clk_ct = 0, csr = 0, csr_ct = 0, cpu = 0;
+	struct sdpm_clk_instance *sdpm_clk;
+	struct device_node *dev_node = dev->of_node;
+	struct device_node *cpu_phandle = NULL;
+	struct device *cpu_dev;
+	struct dev_pm_opp *opp = NULL;
+	unsigned long freq;
+	struct resource *res;
+
+	sdpm_clk = devm_kzalloc(dev, sizeof(*sdpm_clk), GFP_KERNEL);
+	if (!sdpm_clk)
+		return -ENOMEM;
+	sdpm_clk->dev = dev;
+
+	res = platform_get_resource(pdev, IORESOURCE_MEM, 0);
+	if (!res) {
+		dev_err(dev, "Couldn't get MEM resource\n");
+		return -EINVAL;
+	}
+	dev_dbg(dev, "sdpm@0x%x size:%d\n", res->start,
+			resource_size(res));
+	dev_set_drvdata(dev, sdpm_clk);
+
+	sdpm_clk->regmap = devm_ioremap_resource(dev, res);
+	if (!sdpm_clk->regmap) {
+		dev_err(dev, "Couldn't get regmap\n");
+		return -EINVAL;
+	}
+
+	ret = of_property_count_strings(dev_node, "clock-names");
+	if (ret < 0) {
+		dev_err(dev, "Couldn't get clock names. %d\n", ret);
+		return ret;
+	}
+	clk_ct = ret;
+	ret = of_property_count_u32_elems(dev_node, "csr-id");
+	if (ret <= 0) {
+		dev_err(dev, "Couldn't get csr ID array. %d\n", ret);
+		return ret;
+	}
+	csr_ct = ret;
+	if (clk_ct > csr_ct || (csr_ct - clk_ct > 1)) {
+		dev_err(dev, "Invalid csr:%d and clk:%d count.\n", csr_ct,
+				clk_ct);
+		return -EINVAL;
+	}
+	sdpm_clk->clk_ct = clk_ct;
+	sdpm_clk->clk_data = devm_kcalloc(dev, clk_ct + 1,
+				sizeof(*sdpm_clk->clk_data), GFP_KERNEL);
+	if (!sdpm_clk->clk_data)
+		return -ENOMEM;
+
+	for (idx = 0; idx < sdpm_clk->clk_ct; idx++) {
+		ret = of_property_read_string_index(dev_node, "clock-names",
+				idx, &sdpm_clk->clk_data[idx].clock_name);
+		if (ret < 0) {
+			dev_err(dev, "Couldn't get clk name index:%d. %d\n",
+					idx, ret);
+			return ret;
+		}
+
+		sdpm_clk->clk_data[idx].clk = devm_clk_get(dev,
+				sdpm_clk->clk_data[idx].clock_name);
+		if (IS_ERR(sdpm_clk->clk_data[idx].clk))
+			return PTR_ERR(sdpm_clk->clk_data[idx].clk);
+
+		ret = of_property_read_u32_index(dev_node, "csr-id", idx, &csr);
+		if (ret < 0) {
+			dev_err(dev, "Couldn't get CSR for index:%d. %d\n",
+					idx, ret);
+			return ret;
+		}
+		if (ret > CSR_MAX_VAL) {
+			dev_err(dev, "Invalid CSR %d\n", csr);
+			return -EINVAL;
+		}
+		dev_dbg(dev, "SDPM clock:%s csr:%d initialized\n",
+				sdpm_clk->clk_data[idx].clock_name, csr);
+		sdpm_clk->clk_data[idx].csr_id = csr;
+		sdpm_clk->clk_data[idx].sdpm_inst = sdpm_clk;
+		sdpm_clk->clk_data[idx].clk_rate_nb.notifier_call =
+			sdpm_clock_notifier;
+		sdpm_clk->clk_data[idx].last_freq = FREQ_HZ_TO_MHZ(
+				clk_get_rate(sdpm_clk->clk_data[idx].clk));
+		sdpm_csr_write(&sdpm_clk->clk_data[idx],
+				sdpm_clk->clk_data[idx].last_freq);
+		clk_notifier_register(sdpm_clk->clk_data[idx].clk,
+					&sdpm_clk->clk_data[idx].clk_rate_nb);
+	}
+	cpu_phandle = of_parse_phandle(dev_node, "cpu", 0);
+	if (!cpu_phandle)
+		return 0;
+
+	for_each_possible_cpu(cpu) {
+		cpu_dev = get_cpu_device(cpu);
+		if (!cpu_dev || cpu_dev->of_node != cpu_phandle)
+			continue;
+		sdpm_clk->clk_data[idx].clk = NULL;
+		sdpm_clk->clk_data[idx].clock_name = NULL;
+		ret = of_property_read_u32_index(dev_node, "csr-id", idx,
+							&csr);
+		if (ret < 0) {
+			dev_err(dev, "Couldn't get CSR for index:%d. %d\n",
+					idx, ret);
+			return ret;
+		}
+		if (ret > CSR_MAX_VAL) {
+			dev_err(dev, "Invalid CSR %d\n", csr);
+			return -EINVAL;
+		}
+		dev_dbg(dev, "CPU%d clock csr:%d initialized\n",
+				cpu, csr);
+		sdpm_clk->clk_data[idx].csr_id = csr;
+		sdpm_clk->clk_data[idx].cpu_id = cpu;
+		sdpm_clk->clk_data[idx].sdpm_inst = sdpm_clk;
+		sdpm_clk->clk_data[idx].clk_rate_nb.notifier_call =
+			sdpm_cpu_notifier;
+		freq = UINT_MAX;
+		opp = dev_pm_opp_find_freq_floor(cpu_dev, &freq);
+		if (IS_ERR(opp)) {
+			dev_err(dev, "OPP for CPU%d fetch error:%d\n", cpu,
+					PTR_ERR(opp));
+			return PTR_ERR(opp);
+		}
+		sdpm_clk->clk_data[idx].last_freq = FREQ_HZ_TO_MHZ(freq);
+		sdpm_csr_write(&sdpm_clk->clk_data[idx],
+				sdpm_clk->clk_data[idx].last_freq);
+		dev_pm_opp_put(opp);
+		cpufreq_register_notifier(&sdpm_clk->clk_data[idx].clk_rate_nb,
+					CPUFREQ_TRANSITION_NOTIFIER);
+		break;
+	}
+
+	return 0;
+}
+
+static int sdpm_clk_device_remove(struct platform_device *pdev)
+{
+	struct sdpm_clk_instance *sdpm_clk =
+		(struct sdpm_clk_instance *)dev_get_drvdata(&pdev->dev);
+	int idx = 0;
+
+	for (idx = 0; idx < sdpm_clk->clk_ct; idx++) {
+		clk_notifier_unregister(sdpm_clk->clk_data[idx].clk,
+					&sdpm_clk->clk_data[idx].clk_rate_nb);
+	}
+	cpufreq_unregister_notifier(&sdpm_clk->clk_data[idx].clk_rate_nb,
+					CPUFREQ_TRANSITION_NOTIFIER);
+
+	return 0;
+}
+
+static const struct of_device_id sdpm_clk_device_match[] = {
+	{.compatible = "qcom,sdpm"},
+	{}
+};
+
+static struct platform_driver sdpm_clk_device_driver = {
+	.probe          = sdpm_clk_device_probe,
+	.remove         = sdpm_clk_device_remove,
+	.driver         = {
+		.name   = SDPM_DRIVER,
+		.of_match_table = sdpm_clk_device_match,
+	},
+};
+
+module_platform_driver(sdpm_clk_device_driver);
+MODULE_LICENSE("GPL v2");