src/kernel/linux/v4.19/drivers/interconnect/mediatek/mmqos-mtk.c

// SPDX-License-Identifier: GPL-2.0
/*
 * Copyright (c) 2019 MediaTek Inc.
 * Author: Ming-Fan Chen <ming-fan.chen@mediatek.com>
 */

#include <dt-bindings/interconnect/mtk,mmqos.h>
#include <linux/clk.h>
#include <linux/interconnect-provider.h>
#include <linux/kthread.h>
#include <linux/module.h>
#include <linux/of_platform.h>
#include <linux/platform_device.h>
#include <linux/soc/mediatek/mtk_mmdvfs.h>
#include <soc/mediatek/smi.h>

#include "mmqos-mtk.h"

#define SHIFT_ROUND(a, b)	((((a) - 1) >> (b)) + 1)
#define icc_to_MBps(x)	((x) / 1000)

static void mmqos_update_comm_bw(struct device *dev,
	u32 comm_port, u32 freq, u64 mix_bw, u64 bw_peak, bool qos_bound)
{
	u32 comm_bw = 0;
	u32 value;

	if (!freq || !dev)
		return;

	if (mix_bw)
		comm_bw = (mix_bw << 8) / freq;

	if (comm_bw)
		value = ((comm_bw > 0xfff) ? 0xfff : comm_bw) |
			((bw_peak > 0 || !qos_bound) ? 0x1000 : 0x3000);
	else
		value = 0x1200;

	mtk_smi_common_bw_set(dev, comm_port, value);

	dev_dbg(dev, "comm port=%d bw=%d freq=%d qos_bound=%d value=%#x\n",
		comm_port, comm_bw, freq, qos_bound, value);
}

static int update_mm_clk(struct notifier_block *nb,
		unsigned long value, void *v)
{
	struct mtk_mmqos *mmqos =
		container_of(nb, struct mtk_mmqos, nb);
	struct common_node *comm_node;
	struct common_port_node *comm_port;

	list_for_each_entry(comm_node, &mmqos->comm_list, list) {
		comm_node->freq = clk_get_rate(comm_node->clk)/1000000;
		list_for_each_entry(comm_port,
				    &comm_node->comm_port_list, list) {
			mutex_lock(&comm_port->bw_lock);
			if (comm_port->latest_mix_bw
				|| comm_port->latest_peak_bw) {
				mmqos_update_comm_bw(comm_port->larb_dev,
					comm_port->base->icc_node->id & 0xff,
					comm_port->common->freq,
					icc_to_MBps(comm_port->latest_mix_bw),
					icc_to_MBps(comm_port->latest_peak_bw),
					mmqos->qos_bound);
			}
			mutex_unlock(&comm_port->bw_lock);
		}
	}
	return 0;
}

static void set_comm_icc_bw_handler(struct work_struct *work)
{
	struct common_node *comm_node = container_of(
				work, struct common_node, work);
	struct common_port_node *comm_port_node;
	u32 avg_bw = 0, peak_bw = 0;

	list_for_each_entry(comm_port_node, &comm_node->comm_port_list, list) {
		mutex_lock(&comm_port_node->bw_lock);
		avg_bw += comm_port_node->latest_avg_bw;
		peak_bw += (comm_port_node->latest_peak_bw
				& ~(MTK_MMQOS_MAX_BW));
		mutex_unlock(&comm_port_node->bw_lock);
	}
	icc_set_bw(comm_node->icc_path, avg_bw, peak_bw);
}

static int mtk_mmqos_set(struct icc_node *src, struct icc_node *dst)
{
	struct larb_node *larb_node;
	struct larb_port_node *larb_port_node;
	struct common_port_node *comm_port_node;
	struct common_node *comm_node;
	struct mtk_mmqos *mmqos = container_of(dst->provider,
					struct mtk_mmqos, prov);
	u32 value = 1;

	switch (dst->id >> 16) {
	case MTK_MMQOS_NODE_COMMON:
		comm_node = (struct common_node *)dst->data;
		queue_work(mmqos->wq, &comm_node->work);
		break;
	case MTK_MMQOS_NODE_COMMON_PORT:
		comm_port_node = (struct common_port_node *)dst->data;
		mutex_lock(&comm_port_node->bw_lock);
		comm_port_node->latest_mix_bw = comm_port_node->base->mix_bw;
		comm_port_node->latest_peak_bw = dst->peak_bw;
		comm_port_node->latest_avg_bw = dst->avg_bw;
		mmqos_update_comm_bw(comm_port_node->larb_dev,
			dst->id & 0xff, comm_port_node->common->freq,
			icc_to_MBps(comm_port_node->latest_mix_bw),
			icc_to_MBps(comm_port_node->latest_peak_bw),
			mmqos->qos_bound);
		mutex_unlock(&comm_port_node->bw_lock);
		break;
	case MTK_MMQOS_NODE_LARB:
		larb_port_node = (struct larb_port_node *)src->data;
		larb_node = (struct larb_node *)dst->data;
		if (larb_port_node->base->mix_bw)
			value = SHIFT_ROUND(
				icc_to_MBps(larb_port_node->base->mix_bw),
				larb_port_node->bw_ratio);
		if (value > mmqos->max_ratio)
			value = mmqos->max_ratio;
		mtk_smi_larb_bw_set(
			larb_node->larb_dev,
			src->id & 0xff, value);

		if ((dst->id & 0xff) == 1) {
			mtk_smi_larb_bw_set(
				larb_node->larb_dev, 9, 8);
			mtk_smi_larb_bw_set(
				larb_node->larb_dev, 11, 8);
		}
		break;
	default:
		break;
	}
	return 0;
}

static int mtk_mmqos_aggregate(struct icc_node *node,
	u32 avg_bw, u32 peak_bw, u32 *agg_avg, u32 *agg_peak)
{
	struct mmqos_base_node *base_node = NULL;
	u32 mix_bw = peak_bw;

	switch (node->id >> 16) {
	case MTK_MMQOS_NODE_LARB_PORT:
		base_node = ((struct larb_node *)node->data)->base;
		if (peak_bw)
			mix_bw = SHIFT_ROUND(peak_bw * 3, 1);
		break;
	case MTK_MMQOS_NODE_COMMON_PORT:
		base_node = ((struct common_port_node *)node->data)->base;
		break;
	default:
		return 0;
	}

	if (base_node) {
		if (*agg_avg == 0 && *agg_peak == 0)
			base_node->mix_bw = 0;
		base_node->mix_bw += peak_bw ? mix_bw : avg_bw;
	}

	*agg_avg += avg_bw;
	if (peak_bw == MTK_MMQOS_MAX_BW)
		*agg_peak |= MTK_MMQOS_MAX_BW;
	else
		*agg_peak += peak_bw;
	return 0;
}

static struct icc_node *mtk_mmqos_xlate(
	struct of_phandle_args *spec, void *data)
{
	struct icc_onecell_data *icc_data;
	s32 i;

	if (!spec || !data)
		return ERR_PTR(-EPROBE_DEFER);

	icc_data = (struct icc_onecell_data *)data;

	for (i = 0; i < icc_data->num_nodes; i++)
		if (icc_data->nodes[i]->id == spec->args[0])
			return icc_data->nodes[i];

	pr_notice("%s: invalid index %u\n", __func__, spec->args[0]);
	return ERR_PTR(-EINVAL);
}

int mtk_mmqos_probe(struct platform_device *pdev)
{
	struct mtk_mmqos *mmqos;
	struct of_phandle_iterator it;
	struct icc_onecell_data *data;
	struct icc_node *node, *temp;
	struct mmqos_base_node *base_node;
	struct common_node *comm_node;
	struct common_port_node *comm_port_node;
	struct larb_node *larb_node;
	struct larb_port_node *larb_port_node;
	struct mtk_smi_iommu smi_imu;
	int i, id, num_larbs = 0, ret;
	const struct mtk_mmqos_desc *mmqos_desc;
	const struct mtk_node_desc *node_desc;
	struct device *larb_dev;
	struct mmqos_hrt *hrt;

	mmqos = devm_kzalloc(&pdev->dev, sizeof(*mmqos), GFP_KERNEL);
	if (!mmqos)
		return -ENOMEM;
	mmqos->dev = &pdev->dev;

	of_for_each_phandle(
		&it, ret, pdev->dev.of_node, "mediatek,larbs", NULL, 0) {
		struct device_node *np;
		struct platform_device *larb_pdev;

		np = of_node_get(it.node);
		if (!of_device_is_available(np))
			continue;

		larb_pdev = of_find_device_by_node(np);
		if (!larb_pdev) {
			larb_pdev = of_platform_device_create(
				np, NULL, platform_bus_type.dev_root);
			if (!larb_pdev || !larb_pdev->dev.driver) {
				of_node_put(np);
				return -EPROBE_DEFER;
			}
		}

		if (of_property_read_u32(np, "mediatek,larb-id", &id))
			id = num_larbs;
		smi_imu.larb_imu[id].dev = &larb_pdev->dev;
		num_larbs += 1;
	}

	INIT_LIST_HEAD(&mmqos->comm_list);

	INIT_LIST_HEAD(&mmqos->prov.nodes);
	mmqos->prov.set = mtk_mmqos_set;
	mmqos->prov.aggregate = mtk_mmqos_aggregate;
	mmqos->prov.xlate = mtk_mmqos_xlate;
	mmqos->prov.dev = &pdev->dev;

	ret = icc_provider_add(&mmqos->prov);
	if (ret) {
		dev_notice(&pdev->dev, "icc_provider_add failed:%d\n", ret);
		return ret;
	}

	mmqos_desc = (struct mtk_mmqos_desc *)
		of_device_get_match_data(&pdev->dev);
	if (!mmqos_desc)
		return -EINVAL;

	data = devm_kzalloc(&pdev->dev,
		sizeof(*data) + mmqos_desc->num_nodes * sizeof(node),
		GFP_KERNEL);
	if (!data)
		return -ENOMEM;

	for (i = 0; i < mmqos_desc->num_nodes; i++) {
		node_desc = &mmqos_desc->nodes[i];
		node = icc_node_create(node_desc->id);
		if (IS_ERR(node)) {
			ret = PTR_ERR(node);
			goto err;
		}
		icc_node_add(node, &mmqos->prov);

		if (node_desc->link != MMQOS_NO_LINK) {
			ret = icc_link_create(node, node_desc->link);
			if (ret)
				goto err;
		}
		node->name = node_desc->name;

		base_node = devm_kzalloc(
			&pdev->dev, sizeof(*base_node), GFP_KERNEL);
		if (!base_node) {
			ret = -ENOMEM;
			goto err;
		}
		base_node->icc_node = node;

		switch (node->id >> 16) {
		case MTK_MMQOS_NODE_COMMON:
			comm_node = devm_kzalloc(
				&pdev->dev, sizeof(*comm_node), GFP_KERNEL);
			if (!comm_node) {
				ret = -ENOMEM;
				goto err;
			}
			INIT_WORK(&comm_node->work, set_comm_icc_bw_handler);
			comm_node->clk = devm_clk_get(&pdev->dev,
				mmqos_desc->comm_muxes[node->id & 0xff]);
			if (IS_ERR(comm_node->clk)) {
				dev_notice(&pdev->dev, "get clk fail:%s\n",
					mmqos_desc->comm_muxes[
						node->id & 0xff]);
				ret = -EINVAL;
				goto err;
			}
			comm_node->freq = clk_get_rate(comm_node->clk)/1000000;
			INIT_LIST_HEAD(&comm_node->list);
			list_add_tail(&comm_node->list, &mmqos->comm_list);
			INIT_LIST_HEAD(&comm_node->comm_port_list);
			comm_node->icc_path = of_icc_get(&pdev->dev,
				mmqos_desc->comm_icc_path_names[
						node->id & 0xff]);
			if (IS_ERR_OR_NULL(comm_node->icc_path)) {
				dev_notice(&pdev->dev,
					"get icc_path fail:%s\n",
					mmqos_desc->comm_icc_path_names[
						node->id & 0xff]);
				ret = -EINVAL;
				goto err;
			}
			comm_node->base = base_node;
			node->data = (void *)comm_node;
			break;
		case MTK_MMQOS_NODE_COMMON_PORT:
			comm_port_node = devm_kzalloc(&pdev->dev,
				sizeof(*comm_port_node), GFP_KERNEL);
			if (!comm_port_node) {
				ret = -ENOMEM;
				goto err;
			}
			mutex_init(&comm_port_node->bw_lock);
			comm_port_node->common = node->links[0]->data;
			INIT_LIST_HEAD(&comm_port_node->list);
			list_add_tail(&comm_port_node->list,
				      &comm_port_node->common->comm_port_list);
			comm_port_node->base = base_node;
			node->data = (void *)comm_port_node;
			break;
		case MTK_MMQOS_NODE_LARB:
			larb_node = devm_kzalloc(
				&pdev->dev, sizeof(*larb_node), GFP_KERNEL);
			if (!larb_node) {
				ret = -ENOMEM;
				goto err;
			}
			comm_port_node = node->links[0]->data;
			larb_dev = smi_imu.larb_imu[node->id &
					(MTK_LARB_NR_MAX-1)].dev;
			if (larb_dev) {
				comm_port_node->larb_dev = larb_dev;
				larb_node->larb_dev = larb_dev;
			}
			larb_node->base = base_node;
			node->data = (void *)larb_node;
			break;
		case MTK_MMQOS_NODE_LARB_PORT:
			larb_port_node = devm_kzalloc(&pdev->dev,
				sizeof(*larb_port_node), GFP_KERNEL);
			if (!larb_port_node) {
				ret = -ENOMEM;
				goto err;
			}
			larb_port_node->bw_ratio = node_desc->bw_ratio;
			larb_port_node->base = base_node;
			node->data = (void *)larb_port_node;
			break;
		default:
			dev_notice(&pdev->dev,
				"invalid node id:%#x\n", node->id);
			ret = -EINVAL;
			goto err;
		}
		data->nodes[i] = node;
	}

	data->num_nodes = mmqos_desc->num_nodes;
	mmqos->prov.data = data;
	mmqos->max_ratio = mmqos_desc->max_ratio;

	mmqos->wq = create_singlethread_workqueue("mmqos_work_queue");
	if (!mmqos->wq) {
		dev_notice(&pdev->dev, "work queue create fail\n");
		ret = -ENOMEM;
		goto err;
	}

	hrt = devm_kzalloc(&pdev->dev, sizeof(*hrt), GFP_KERNEL);
	if (!hrt) {
		ret = -ENOMEM;
		goto err;
	}
	memcpy(hrt, &mmqos_desc->hrt, sizeof(mmqos_desc->hrt));
	mtk_mmqos_init_hrt(hrt);

	mmqos->nb.notifier_call = update_mm_clk;
	register_mmdvfs_notifier(&mmqos->nb);

	ret = mtk_mmqos_register_hrt_sysfs(&pdev->dev);
	if (ret)
		dev_notice(&pdev->dev, "sysfs create fail\n");

	platform_set_drvdata(pdev, mmqos);

	return 0;

err:
	list_for_each_entry_safe(node, temp, &mmqos->prov.nodes, node_list) {
		icc_node_del(node);
		icc_node_destroy(node->id);
	}
	icc_provider_del(&mmqos->prov);
	return ret;
}
EXPORT_SYMBOL_GPL(mtk_mmqos_probe);

int mtk_mmqos_remove(struct platform_device *pdev)
{
	struct mtk_mmqos *mmqos = platform_get_drvdata(pdev);
	struct icc_node *node, *temp;

	list_for_each_entry_safe(node, temp, &mmqos->prov.nodes, node_list) {
		icc_node_del(node);
		icc_node_destroy(node->id);
	}
	icc_provider_del(&mmqos->prov);
	unregister_mmdvfs_notifier(&mmqos->nb);
	destroy_workqueue(mmqos->wq);
	mtk_mmqos_unregister_hrt_sysfs(&pdev->dev);
	return 0;
}
EXPORT_SYMBOL_GPL(mtk_mmqos_remove);

MODULE_LICENSE("GPL v2");