interconnect: qcom: Add QoS config support

Introduce support to initialize QoS settings for
QNOC platforms.

Change-Id: I17a237e88f21f7bbc98621d072fff9c1b1bc6403
Signed-off-by: David Dai <daidavid1@codeaurora.org>
Signed-off-by: Odelu Kukatla <okukatla@codeaurora.org>
Signed-off-by: Mike Tipton <mdtipton@codeaurora.org>

drivers/interconnect/qcom/Kconfig

@@ -175,3 +175,6 @@ config INTERCONNECT_QCOM_SM8450
 
 config INTERCONNECT_QCOM_SMD_RPM
 	tristate
+
+config INTERCONNECT_QCOM_QOS
+	tristate

drivers/interconnect/qcom/Makefile

@@ -20,6 +20,7 @@ qnoc-sm8250-objs			:= sm8250.o
 qnoc-sm8350-objs			:= sm8350.o
 qnoc-sm8450-objs			:= sm8450.o
 icc-smd-rpm-objs			:= smd-rpm.o icc-rpm.o
+qnoc-qos-obj				:= qnoc-qos.o
 
 obj-$(CONFIG_INTERCONNECT_QCOM_BCM_VOTER) += icc-bcm-voter.o
 obj-$(CONFIG_INTERCONNECT_QCOM_MSM8916) += qnoc-msm8916.o
@@ -41,3 +42,4 @@ obj-$(CONFIG_INTERCONNECT_QCOM_SM8250) += qnoc-sm8250.o
 obj-$(CONFIG_INTERCONNECT_QCOM_SM8350) += qnoc-sm8350.o
 obj-$(CONFIG_INTERCONNECT_QCOM_SM8450) += qnoc-sm8450.o
 obj-$(CONFIG_INTERCONNECT_QCOM_SMD_RPM) += icc-smd-rpm.o
+obj-$(CONFIG_INTERCONNECT_QCOM_QOS) += qnoc-qos.o

drivers/interconnect/qcom/icc-rpmh.c

@@ -3,6 +3,7 @@
  * Copyright (c) 2020, The Linux Foundation. All rights reserved.
  */
 
+#include <linux/clk.h>
 #include <linux/interconnect.h>
 #include <linux/interconnect-provider.h>
 #include <linux/module.h>
@@ -13,6 +14,7 @@
 
 #include "bcm-voter.h"
 #include "icc-rpmh.h"
+#include "qnoc-qos.h"
 
 static LIST_HEAD(qnoc_probe_list);
 static DEFINE_MUTEX(probe_list_lock);
@@ -187,6 +189,24 @@ int qcom_icc_bcm_init(struct qcom_icc_bcm *bcm, struct device *dev)
 }
 EXPORT_SYMBOL_GPL(qcom_icc_bcm_init);
 
+static struct regmap *qcom_icc_rpmh_map(struct platform_device *pdev,
+					const struct qcom_icc_desc *desc)
+{
+	void __iomem *base;
+	struct resource *res;
+	struct device *dev = &pdev->dev;
+
+	res = platform_get_resource(pdev, IORESOURCE_MEM, 0);
+	if (!res)
+		return NULL;
+
+	base = devm_ioremap_resource(dev, res);
+	if (IS_ERR(base))
+		return ERR_CAST(base);
+
+	return devm_regmap_init_mmio(dev, base, desc->config);
+}
+
 int qcom_icc_rpmh_probe(struct platform_device *pdev)
 {
 	const struct qcom_icc_desc *desc;
@@ -231,10 +251,24 @@ int qcom_icc_rpmh_probe(struct platform_device *pdev)
 	if (IS_ERR(qp->voter))
 		return PTR_ERR(qp->voter);
 
+	qp->regmap = qcom_icc_rpmh_map(pdev, desc);
+	if (IS_ERR(qp->regmap))
+		return PTR_ERR(qp->regmap);
+
 	ret = icc_provider_add(provider);
 	if (ret)
 		return ret;
 
+	qp->num_clks = devm_clk_bulk_get_all(qp->dev, &qp->clks);
+	if (qp->num_clks < 0)
+		return qp->num_clks;
+
+	ret = clk_bulk_prepare_enable(qp->num_clks, qp->clks);
+	if (ret) {
+		dev_err(&pdev->dev, "failed to enable clocks\n");
+		return ret;
+	}
+
 	for (i = 0; i < qp->num_bcms; i++)
 		qcom_icc_bcm_init(qp->bcms[i], dev);
 
@@ -243,12 +277,17 @@ int qcom_icc_rpmh_probe(struct platform_device *pdev)
 		if (!qn)
 			continue;
 
+		qn->regmap = dev_get_regmap(qp->dev, NULL);
+
 		node = icc_node_create(qn->id);
 		if (IS_ERR(node)) {
 			ret = PTR_ERR(node);
 			goto err;
 		}
 
+		if (qn->qosbox)
+			qn->noc_ops->set_qos(qn);
+
 		node->name = qn->name;
 		node->data = qn;
 		icc_node_add(node, provider);
@@ -268,6 +307,8 @@ int qcom_icc_rpmh_probe(struct platform_device *pdev)
 
 	return 0;
 err:
+	clk_bulk_disable_unprepare(qp->num_clks, qp->clks);
+	clk_bulk_put_all(qp->num_clks, qp->clks);
 	icc_nodes_remove(provider);
 	icc_provider_del(provider);
 	return ret;
@@ -278,6 +319,8 @@ int qcom_icc_rpmh_remove(struct platform_device *pdev)
 {
 	struct qcom_icc_provider *qp = platform_get_drvdata(pdev);
 
+	clk_bulk_put_all(qp->num_clks, qp->clks);
+
 	icc_nodes_remove(&qp->provider);
 	return icc_provider_del(&qp->provider);
 }

drivers/interconnect/qcom/icc-rpmh.h

@@ -7,6 +7,8 @@
 #define __DRIVERS_INTERCONNECT_QCOM_ICC_RPMH_H__
 
 #include <dt-bindings/interconnect/qcom,icc.h>
+#include <linux/regmap.h>
+#include <linux/platform_device.h>
 
 #define to_qcom_provider(_provider) \
 	container_of(_provider, struct qcom_icc_provider, provider)
@@ -26,6 +28,9 @@ struct qcom_icc_provider {
 	size_t num_bcms;
 	struct bcm_voter *voter;
 	struct list_head probe_list;
+	struct regmap *regmap;
+	struct clk_bulk_data *clks;
+	int num_clks;
 };
 
 /**
@@ -71,6 +76,9 @@ struct qcom_icc_node {
 	u64 max_peak[QCOM_ICC_NUM_BUCKETS];
 	struct qcom_icc_bcm *bcms[MAX_BCM_PER_NODE];
 	size_t num_bcms;
+	struct regmap *regmap;
+	struct qcom_icc_qosbox *qosbox;
+	const struct qcom_icc_noc_ops *noc_ops;
 };
 
 /**
@@ -113,6 +121,7 @@ struct qcom_icc_fabric {
 };
 
 struct qcom_icc_desc {
+	const struct regmap_config *config;
 	struct qcom_icc_node **nodes;
 	size_t num_nodes;
 	struct qcom_icc_bcm **bcms;

drivers/interconnect/qcom/qnoc-qos.c

@@ -0,0 +1,65 @@
+// SPDX-License-Identifier: GPL-2.0-only
+/*
+ * Copyright (c) 2020, The Linux Foundation. All rights reserved.
+ *
+ */
+
+#include <linux/interconnect.h>
+#include <linux/interconnect-provider.h>
+#include <linux/module.h>
+
+#include "icc-rpmh.h"
+#include "qnoc-qos.h"
+
+#define QOSGEN_MAINCTL_LO(p, qp)	((p)->offsets[qp] + \
+					(p)->regs[QOSGEN_OFF_MAINCTL_LO])
+# define QOS_SLV_URG_MSG_EN		BIT(3)
+# define QOS_DFLT_PRIO_MASK		0x7
+# define QOS_DFLT_PRIO_SHFT		4
+
+const u8 icc_qnoc_qos_regs[][QOSGEN_OFF_MAX_REGS] = {
+	[ICC_QNOC_QOSGEN_TYPE_RPMH] = {
+		[QOSGEN_OFF_MAINCTL_LO] = 0x8,
+		[QOSGEN_OFF_LIMITBW_LO] = 0x18,
+		[QOSGEN_OFF_SHAPING_LO] = 0x20,
+		[QOSGEN_OFF_SHAPING_HI] = 0x24,
+		[QOSGEN_OFF_REGUL0CTL_LO] = 0x40,
+		[QOSGEN_OFF_REGUL0BW_LO] = 0x48,
+	},
+};
+EXPORT_SYMBOL(icc_qnoc_qos_regs);
+
+/**
+ * qcom_icc_set_qos - initialize static QoS configurations
+ * @node: qcom icc node to operate on
+ */
+static void qcom_icc_set_qos(struct qcom_icc_node *node)
+{
+	struct qcom_icc_qosbox *qos = node->qosbox;
+	int port;
+
+	if (!node->regmap)
+		return;
+
+	if (!qos)
+		return;
+
+	for (port = 0; port < qos->num_ports; port++) {
+		regmap_update_bits(node->regmap, QOSGEN_MAINCTL_LO(qos, port),
+				   QOS_DFLT_PRIO_MASK << QOS_DFLT_PRIO_SHFT,
+				   qos->config->prio << QOS_DFLT_PRIO_SHFT);
+
+		if (qos->config->urg_fwd)
+			regmap_update_bits(node->regmap,
+					   QOSGEN_MAINCTL_LO(qos, port),
+					   QOS_SLV_URG_MSG_EN,
+					   QOS_SLV_URG_MSG_EN);
+	}
+}
+
+const struct qcom_icc_noc_ops qcom_qnoc4_ops = {
+	.set_qos = qcom_icc_set_qos,
+};
+EXPORT_SYMBOL(qcom_qnoc4_ops);
+
+MODULE_LICENSE("GPL v2");

drivers/interconnect/qcom/qnoc-qos.h

@@ -0,0 +1,58 @@
+/* SPDX-License-Identifier: GPL-2.0-only */
+/*
+ * Copyright (c) 2020, The Linux Foundation. All rights reserved.
+ *
+ */
+
+#ifndef __DRIVERS_INTERCONNECT_QCOM_QNOC_QOS_H__
+#define __DRIVERS_INTERCONNECT_QCOM_QNOC_QOS_H__
+
+#define QOSGEN_OFF_MAX_REGS 6
+#define ICC_QNOC_QOS_MAX_TYPE 1
+
+enum {
+	ICC_QNOC_QOSGEN_TYPE_RPMH,
+};
+
+enum {
+	QOSGEN_OFF_MAINCTL_LO,
+	QOSGEN_OFF_LIMITBW_LO,
+	QOSGEN_OFF_SHAPING_LO,
+	QOSGEN_OFF_SHAPING_HI,
+	QOSGEN_OFF_REGUL0CTL_LO,
+	QOSGEN_OFF_REGUL0BW_LO,
+};
+
+extern const u8 icc_qnoc_qos_regs[ICC_QNOC_QOS_MAX_TYPE][QOSGEN_OFF_MAX_REGS];
+
+struct qcom_icc_noc_ops {
+	void		(*set_qos)(struct qcom_icc_node *node);
+};
+
+struct qos_config {
+	u32 prio;
+	u32 urg_fwd;
+};
+
+struct qcom_icc_qosbox {
+	u32 num_ports;
+	const u8 *regs;
+	struct qos_config *config;
+	u32 offsets[];
+};
+
+#define DEFINE_QNODE_QOS(_name, _prio, _urg_fwd, _num_ports, ...)	\
+		static struct qos_config _name##_qos_cfg = {		\
+		.prio = _prio,						\
+		.urg_fwd = _urg_fwd,					\
+	};								\
+		static struct qcom_icc_qosbox _name##_qos = {		\
+		.num_ports = _num_ports,				\
+		.config = &_name##_qos_cfg,				\
+		.regs = icc_qnoc_qos_regs[ICC_QNOC_QOSGEN_TYPE_RPMH],	\
+		.offsets = {__VA_ARGS__},				\
+	}								\
+
+extern const struct qcom_icc_noc_ops qcom_qnoc4_ops;
+
+#endif