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 <[email protected]> Signed-off-by: Manaf Meethalavalappu Pallikunhi <[email protected]> --- 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
