[RFC PATCH 2/2] drivers: soc: qcom: Add SDPM clock monitor driver

[Manaf Meethalavalappu Pallikunhi <manafm@codeaurora.org>]

Add SDPM clock monitor driver, which will register for clock rate
change notification and write the clock rate into SDPM CSR register.

Signed-off-by: Ram Chandrasekar <rkumbako@codeaurora.org>
Signed-off-by: Manaf Meethalavalappu Pallikunhi <manafm@codeaurora.org>
---
 drivers/soc/qcom/Kconfig            |   8 ++
 drivers/soc/qcom/Makefile           |   1 +
 drivers/soc/qcom/sdpm_clk_monitor.c | 217 ++++++++++++++++++++++++++++++++++++
 3 files changed, 226 insertions(+)
 create mode 100644 drivers/soc/qcom/sdpm_clk_monitor.c

diff --git a/drivers/soc/qcom/Kconfig b/drivers/soc/qcom/Kconfig
index 9464ff4..1f04d69 100644
--- a/drivers/soc/qcom/Kconfig
+++ b/drivers/soc/qcom/Kconfig
@@ -237,4 +237,12 @@ config QCOM_APR
 	  used by audio driver to configure QDSP6
 	  ASM, ADM and AFE modules.
 
+config QCOM_SDPM_CLOCK_MONITOR
+	tristate "Qualcomm SDPM Clock Monitor"
+	depends on COMMON_CLK
+	help
+	    This enables the Qualcomm 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.
 endmenu
diff --git a/drivers/soc/qcom/Makefile b/drivers/soc/qcom/Makefile
index d658a10..4eef767 100644
--- a/drivers/soc/qcom/Makefile
+++ b/drivers/soc/qcom/Makefile
@@ -29,3 +29,4 @@ obj-$(CONFIG_QCOM_RPMPD) += rpmpd.o
 obj-$(CONFIG_QCOM_KRYO_L2_ACCESSORS) +=	kryo-l2-accessors.o
 obj-$(CONFIG_QCOM_LLCC_PERFMON) += llcc_perfmon.o
 obj-$(CONFIG_QCOM_MEMORY_DUMP_V2) += memory_dump_v2.o
+obj-$(CONFIG_QCOM_SDPM_CLOCK_MONITOR) += sdpm_clk_monitor.o
diff --git a/drivers/soc/qcom/sdpm_clk_monitor.c b/drivers/soc/qcom/sdpm_clk_monitor.c
new file mode 100644
index 00000000..1aee119
--- /dev/null
+++ b/drivers/soc/qcom/sdpm_clk_monitor.c
@@ -0,0 +1,217 @@
+// SPDX-License-Identifier: GPL-2.0-only
+/*
+ * Copyright (c) 2021, The Linux Foundation. All rights reserved.
+ */
+
+#define pr_fmt(fmt) "%s " fmt, KBUILD_MODNAME
+
+#include <linux/clk.h>
+#include <linux/err.h>
+#include <linux/module.h>
+#include <linux/of.h>
+#include <linux/of_address.h>
+#include <linux/platform_device.h>
+#include <linux/slab.h>
+
+#define SDPM_DRIVER		"sdpm-clk-monitor"
+#define CSR_MAX_VAL		7
+#define CSR_OFFSET		0xF00
+#define FREQ_HZ_TO_MHZ(f)	DIV_ROUND_UP((f), 1000000)
+#define SDPM_CSR_OFFSET(id)	(CSR_OFFSET + (id * 4))
+
+struct sdpm_clk_instance;
+struct sdpm_clk_data {
+	struct list_head		sdpm_node;
+	struct notifier_block		clk_rate_nb;
+	struct clk			*clk;
+	const char			*clock_name;
+	uint32_t			csr_id;
+	struct sdpm_clk_instance	*sdpm_inst;
+};
+
+struct sdpm_clk_instance {
+	struct device			*dev;
+	void __iomem			*regmap;
+	struct list_head		sdpm_instances;
+};
+
+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;
+
+	writel_relaxed(clk_rate, sdpm_inst->regmap +
+			SDPM_CSR_OFFSET(sdpm_data->csr_id));
+	dev_dbg(sdpm_inst->dev, "clock:%s offset:0x%x frequency:%u\n",
+			sdpm_data->clock_name,
+			SDPM_CSR_OFFSET(sdpm_data->csr_id),
+			clk_rate);
+}
+
+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_remove(struct platform_device *pdev)
+{
+	struct sdpm_clk_instance *sdpm_inst =
+		(struct sdpm_clk_instance *)dev_get_drvdata(&pdev->dev);
+	struct sdpm_clk_data *sdpm_data = NULL, *next = NULL;
+
+	list_for_each_entry_safe(sdpm_data, next,
+			&sdpm_inst->sdpm_instances, sdpm_node) {
+		clk_notifier_unregister(sdpm_data->clk,
+					&sdpm_data->clk_rate_nb);
+		list_del(&sdpm_data->sdpm_node);
+	}
+
+	return 0;
+}
+
+static int sdpm_clk_device_probe(struct platform_device *pdev)
+{
+	struct device *dev = &pdev->dev;
+	struct device_node *dev_node = dev->of_node;
+	int ret = 0, idx = 0, clk_ct = 0, csr = 0, csr_ct = 0;
+	struct sdpm_clk_instance *sdpm_inst = NULL;
+	struct sdpm_clk_data *sdpm_data = NULL;
+	struct resource *res;
+
+	sdpm_inst = devm_kzalloc(dev, sizeof(*sdpm_inst), GFP_KERNEL);
+	if (!sdpm_inst)
+		return -ENOMEM;
+	sdpm_inst->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_inst);
+
+	sdpm_inst->regmap = devm_ioremap_resource(dev, res);
+	if (!sdpm_inst->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 (csr_ct != clk_ct) {
+		dev_err(dev, "Invalid csr:%d and clk:%d count.\n", csr_ct,
+				clk_ct);
+		return -EINVAL;
+	}
+
+	INIT_LIST_HEAD(&sdpm_inst->sdpm_instances);
+
+	for (idx = 0; idx < clk_ct; idx++) {
+
+		sdpm_data = devm_kzalloc(dev, sizeof(*sdpm_data), GFP_KERNEL);
+		if (!sdpm_data) {
+			ret = -ENOMEM;
+			goto clk_err_exit;
+		}
+
+		ret = of_property_read_string_index(dev_node, "clock-names",
+				idx, &sdpm_data->clock_name);
+		if (ret < 0) {
+			dev_err(dev, "Couldn't get clk name index:%d. %d\n",
+					idx, ret);
+			goto clk_err_exit;
+		}
+
+		sdpm_data->clk = devm_clk_get(dev, sdpm_data->clock_name);
+		if (IS_ERR(sdpm_data->clk)) {
+			ret = PTR_ERR(sdpm_data->clk);
+			goto clk_err_exit;
+		}
+
+		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);
+			goto clk_err_exit;
+		}
+
+		if (ret > CSR_MAX_VAL) {
+			dev_err(dev, "Invalid CSR %d\n", csr);
+			ret = -EINVAL;
+			goto clk_err_exit;
+		}
+
+		dev_dbg(dev, "SDPM clock:%s csr:%d initialized\n",
+				sdpm_data->clock_name, csr);
+		sdpm_data->csr_id = csr;
+		sdpm_data->sdpm_inst = sdpm_inst;
+		sdpm_data->clk_rate_nb.notifier_call = sdpm_clock_notifier;
+		sdpm_csr_write(sdpm_data,
+			FREQ_HZ_TO_MHZ(clk_get_rate(sdpm_data->clk)));
+		clk_notifier_register(sdpm_data->clk, &sdpm_data->clk_rate_nb);
+		list_add(&sdpm_data->sdpm_node, &sdpm_inst->sdpm_instances);
+	}
+
+	return 0;
+
+clk_err_exit:
+	sdpm_clk_device_remove(pdev);
+
+	return ret;
+}
+
+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_DESCRIPTION("Qualcomm Technologies, Inc. SDPM Clock Monitor Driver");
+MODULE_LICENSE("GPL v2");
-- 
The Qualcomm Innovation Center, Inc. is a member of the Code Aurora Forum,
a Linux Foundation Collaborative Project