kernel-fxtec-pro1x/drivers/devfreq/devfreq_qcom_fw.c

// SPDX-License-Identifier: GPL-2.0-only
/*
 * Copyright (c) 2018-2020, The Linux Foundation. All rights reserved.
 */

#include <linux/err.h>
#include <linux/errno.h>
#include <linux/init.h>
#include <linux/io.h>
#include <linux/kernel.h>
#include <linux/module.h>
#include <linux/of.h>
#include <linux/of_address.h>
#include <linux/of_platform.h>
#include <linux/platform_device.h>
#include <linux/slab.h>
#include <linux/list.h>
#include <linux/devfreq.h>
#include <linux/pm_opp.h>

#define INIT_HZ				300000000UL
#define XO_HZ				19200000UL
#define FTBL_MAX_ENTRIES		40U
#define FTBL_ROW_SIZE			4

#define SRC_MASK			GENMASK(31, 30)
#define SRC_SHIFT			30
#define MULT_MASK			GENMASK(7, 0)

struct devfreq_qcom_fw {
	void __iomem *perf_base;
	void __iomem *pstate_base;
	struct devfreq_dev_profile dp;
	struct list_head voters;
	struct list_head voter;
	unsigned int index;
};

static DEFINE_SPINLOCK(voter_lock);
static unsigned int ftbl_row_size = FTBL_ROW_SIZE;
static struct device *dev_node;

static int devfreq_qcom_fw_target(struct device *dev, unsigned long *freq,
				  u32 flags)
{
	struct devfreq_qcom_fw *d = dev_get_drvdata(dev), *pd, *v;
	struct devfreq_dev_profile *p = &d->dp;
	unsigned int index;
	unsigned long lflags;
	struct dev_pm_opp *opp;
	void __iomem *perf_base = d->perf_base;

	opp = devfreq_recommended_opp(dev, freq, flags);
	if (!IS_ERR(opp))
		dev_pm_opp_put(opp);
	else
		return PTR_ERR(opp);

	for (index = 0; index < p->max_state; index++)
		if (p->freq_table[index] == *freq)
			break;

	if (index >= p->max_state) {
		dev_err(dev, "Unable to find index for freq (%lu)!\n", *freq);
		return -EINVAL;
	}

	d->index = index;

	spin_lock_irqsave(&voter_lock, lflags);
	/* Voter */
	if (!perf_base) {
		pd = dev_get_drvdata(dev->parent);
		list_for_each_entry(v, &pd->voters, voter)
			index = max(index, v->index);
		perf_base = pd->perf_base;
	}

	writel_relaxed(index, perf_base);
	spin_unlock_irqrestore(&voter_lock, lflags);

	return 0;
}

static int devfreq_panic_callback(struct notifier_block *nfb,
					unsigned long event, void *unused)
{
	struct devfreq_qcom_fw *d = dev_get_drvdata(dev_node);
	void __iomem *base;

	pr_err("%s: L3-DOMAIN\n", __func__);
	base = d->perf_base;
	pr_err("%25s: 0x%.8x\n", "PERF_STATE_DESIRED", readl_relaxed(base));
	base = d->pstate_base;
	pr_err("%25s: 0x%.8x\n", "PSTATE_STATUS", readl_relaxed(base));

	return NOTIFY_OK;
}

static struct notifier_block devfreq_panic_notifier = {
	.notifier_call = devfreq_panic_callback,
	.priority = 1,
};

static int devfreq_qcom_fw_get_cur_freq(struct device *dev,
						 unsigned long *freq)
{
	struct devfreq_qcom_fw *d = dev_get_drvdata(dev);
	struct devfreq_dev_profile *p = &d->dp;
	unsigned int index;

	/* Voter */
	if (!d->perf_base) {
		index = d->index;
	} else {
		index = readl_relaxed(d->perf_base);
		index = min(index, p->max_state - 1);
	}
	*freq = p->freq_table[index];

	return 0;
}

static int devfreq_qcom_populate_opp(struct platform_device *pdev)
{
	struct device *dev = &pdev->dev;
	u32 data, src, mult, i;
	unsigned long freq, prev_freq = 0;
	struct resource *res;
	void __iomem *ftbl_base;

	res = platform_get_resource_byname(pdev, IORESOURCE_MEM, "ftbl-base");
	if (!res) {
		dev_err(dev, "Unable to find ftbl-base!\n");
		return -EINVAL;
	}

	ftbl_base = devm_ioremap(dev, res->start, resource_size(res));
	if (!ftbl_base) {
		dev_err(dev, "Unable to map ftbl-base\n");
		return -ENOMEM;
	}

	of_property_read_u32(pdev->dev.of_node, "qcom,ftbl-row-size",
						     &ftbl_row_size);

	for (i = 0; i < FTBL_MAX_ENTRIES; i++) {
		data = readl_relaxed(ftbl_base + i * ftbl_row_size);
		src = ((data & SRC_MASK) >> SRC_SHIFT);
		mult = (data & MULT_MASK);
		freq = src ? XO_HZ * mult : INIT_HZ;

		/*
		 * Two of the same frequencies with the same core counts means
		 * end of table.
		 */
		if (i > 0 && prev_freq == freq)
			break;

		dev_pm_opp_add(&pdev->dev, freq, 0);

		prev_freq = freq;
	}

	devm_iounmap(dev, ftbl_base);

	return 0;
}

static int devfreq_qcom_init_hw(struct platform_device *pdev)
{
	struct devfreq_qcom_fw *d;
	struct resource *res;
	struct device *dev = &pdev->dev;
	int ret = 0;
	void __iomem *en_base;

	d = devm_kzalloc(dev, sizeof(*d), GFP_KERNEL);
	if (!d)
		return -ENOMEM;

	res = platform_get_resource_byname(pdev, IORESOURCE_MEM, "en-base");
	if (!res) {
		dev_err(dev, "Unable to find en-base!\n");
		return -EINVAL;
	}

	en_base = devm_ioremap(dev, res->start, resource_size(res));
	if (!en_base) {
		dev_err(dev, "Unable to map en-base\n");
		return -ENOMEM;
	}

	/* Firmware should be enabled state to proceed */
	if (!(readl_relaxed(en_base) & 1)) {
		dev_err(dev, "Firmware not enabled\n");
		return -ENODEV;
	}

	devm_iounmap(dev, en_base);

	ret = devfreq_qcom_populate_opp(pdev);
	if (ret) {
		dev_err(dev, "Failed to read FTBL\n");
		return ret;
	}

	res = platform_get_resource_byname(pdev, IORESOURCE_MEM, "perf-base");
	if (!res) {
		dev_err(dev, "Unable to find perf-base!\n");
		ret = -EINVAL;
		goto out;
	}

	d->perf_base = devm_ioremap(dev, res->start, resource_size(res));
	if (!d->perf_base) {
		dev_err(dev, "Unable to map perf-base\n");
		ret = -ENOMEM;
		goto out;
	}

	if (of_property_read_bool(dev->of_node,
					"qcom,support-panic-notifier")) {
		res = platform_get_resource_byname(pdev, IORESOURCE_MEM,
								"pstate-base");
		if (!res) {
			dev_err(dev, "Unable to find pstate-base!\n");
			ret = -EINVAL;
			goto out;
		}

		d->pstate_base = devm_ioremap(dev, res->start,
							resource_size(res));
		if (!d->pstate_base) {
			dev_err(dev, "Unable to map pstate-base\n");
			ret = -ENOMEM;
			goto out;
		}

		atomic_notifier_chain_register(&panic_notifier_list,
						&devfreq_panic_notifier);
		dev_node = dev;
	}

	INIT_LIST_HEAD(&d->voters);
	dev_set_drvdata(dev, d);

out:
	if (ret)
		dev_pm_opp_remove_table(dev);
	return ret;
}

static int devfreq_qcom_copy_opp(struct device *src_dev, struct device *dst_dev)
{
	unsigned long freq;
	int i, cnt, ret = 0;
	struct dev_pm_opp *opp;

	if (!src_dev)
		return -ENODEV;

	cnt = dev_pm_opp_get_opp_count(src_dev);
	if (!cnt)
		return -EINVAL;

	for (i = 0, freq = 0; i < cnt; i++, freq++) {
		opp = dev_pm_opp_find_freq_ceil(src_dev, &freq);
		if (IS_ERR(opp)) {
			ret = -EINVAL;
			break;
		}
		dev_pm_opp_put(opp);

		ret = dev_pm_opp_add(dst_dev, freq, 0);
		if (ret)
			break;
	}

	if (ret)
		dev_pm_opp_remove_table(dst_dev);
	return ret;
}

static int devfreq_qcom_init_voter(struct platform_device *pdev)
{
	struct device *dev = &pdev->dev;
	struct device *par_dev = dev->parent;
	struct devfreq_qcom_fw *d, *pd = dev_get_drvdata(par_dev);
	int ret = 0;

	d = devm_kzalloc(dev, sizeof(*d), GFP_KERNEL);
	if (!d)
		return -ENOMEM;

	ret = devfreq_qcom_copy_opp(dev->parent, dev);
	if (ret) {
		dev_err(dev, "Failed to copy parent OPPs\n");
		return ret;
	}

	list_add(&d->voter, &pd->voters);
	dev_set_drvdata(dev, d);

	return 0;
}

static int devfreq_qcom_fw_driver_probe(struct platform_device *pdev)
{
	struct device *dev = &pdev->dev;
	int ret = 0;
	struct devfreq_qcom_fw *d;
	struct devfreq_dev_profile *p;
	struct devfreq *df;

	if (!of_device_get_match_data(dev))
		ret = devfreq_qcom_init_voter(pdev);
	else
		ret = devfreq_qcom_init_hw(pdev);
	if (ret) {
		dev_err(dev, "Unable to probe device!\n");
		return ret;
	}

	/*
	 * If device has voter children, do no register directly with devfreq
	 */
	if (of_get_available_child_count(dev->of_node)) {
		of_platform_populate(dev->of_node, NULL, NULL, dev);
		dev_info(dev, "Devfreq QCOM Firmware parent dev inited.\n");
		return 0;
	}

	d = dev_get_drvdata(dev);
	p = &d->dp;
	p->polling_ms = 50;
	p->target = devfreq_qcom_fw_target;
	p->get_cur_freq = devfreq_qcom_fw_get_cur_freq;

	df = devm_devfreq_add_device(dev, p, "performance", NULL);
	if (IS_ERR(df)) {
		dev_err(dev, "Unable to register Devfreq QCOM Firmware dev!\n");
		return PTR_ERR(df);
	}

	dev_info(dev, "Devfreq QCOM Firmware dev registered.\n");

	return 0;
}

static const struct of_device_id match_table[] = {
	{ .compatible = "qcom,devfreq-fw", .data = (void *) 1 },
	{ .compatible = "qcom,devfreq-fw-voter", .data = (void *) 0 },
	{}
};

static struct platform_driver devfreq_qcom_fw_driver = {
	.probe = devfreq_qcom_fw_driver_probe,
	.driver = {
		.name = "devfreq-qcom-fw",
		.of_match_table = match_table,
	},
};

static int __init devfreq_qcom_fw_init(void)
{
	return platform_driver_register(&devfreq_qcom_fw_driver);
}
subsys_initcall(devfreq_qcom_fw_init);

static void __exit devfreq_qcom_fw_exit(void)
{
	platform_driver_unregister(&devfreq_qcom_fw_driver);
}
module_exit(devfreq_qcom_fw_exit);

MODULE_DESCRIPTION("Devfreq QCOM Firmware");
MODULE_LICENSE("GPL v2");