layerscape: add patches-5.4

Add patches for linux-5.4. The patches are from NXP LSDK-20.04 release
which was tagged LSDK-20.04-V5.4.
https://source.codeaurora.org/external/qoriq/qoriq-components/linux/

For boards LS1021A-IOT, and Traverse-LS1043 which are not involved in
LSDK, port the dts patches from 4.14.

The patches are sorted into the following categories:
  301-arch-xxxx
  302-dts-xxxx
  303-core-xxxx
  701-net-xxxx
  801-audio-xxxx
  802-can-xxxx
  803-clock-xxxx
  804-crypto-xxxx
  805-display-xxxx
  806-dma-xxxx
  807-gpio-xxxx
  808-i2c-xxxx
  809-jailhouse-xxxx
  810-keys-xxxx
  811-kvm-xxxx
  812-pcie-xxxx
  813-pm-xxxx
  814-qe-xxxx
  815-sata-xxxx
  816-sdhc-xxxx
  817-spi-xxxx
  818-thermal-xxxx
  819-uart-xxxx
  820-usb-xxxx
  821-vfio-xxxx

Signed-off-by: Yangbo Lu <yangbo.lu@nxp.com>

1 files changed, 1619 insertions, 0 deletions

diff --git a/target/linux/layerscape/patches-5.4/701-net-0213-dpaa2-eth-Add-CEETM-qdisc-support.patch b/target/linux/layerscape/patches-5.4/701-net-0213-dpaa2-eth-Add-CEETM-qdisc-support.patch
new file mode 100644
index 0000000000..3e586e457a
--- /dev/null
+++ b/target/linux/layerscape/patches-5.4/701-net-0213-dpaa2-eth-Add-CEETM-qdisc-support.patch
@@ -0,0 +1,1619 @@
+From 68622c8bb029f9fd4c83ffa3bd979fa62a3599d0 Mon Sep 17 00:00:00 2001
+From: Bogdan Purcareata <bogdan.purcareata@nxp.com>
+Date: Mon, 13 Nov 2017 17:26:13 +0200
+Subject: [PATCH] dpaa2-eth: Add CEETM qdisc support
+
+Features include:
+- dual rate shaping support
+- per-channel shaping and classification
+- strict / weighted scheduling among num_tc classes
+- TD enabled for configured class queues
+- prio class (leaf) firmware statistics support
+- weights normalized based on max
+- tc filters based classification
+
+Only 1 CEETM ch supported, only channel shaping supported.
+
+Signed-off-by: Bogdan Purcareata <bogdan.purcareata@nxp.com>
+Signed-off-by: Camelia Groza <camelia.groza@nxp.com>
+---
+ drivers/net/ethernet/freescale/dpaa2/Kconfig       |    7 +
+ drivers/net/ethernet/freescale/dpaa2/Makefile      |    1 +
+ .../net/ethernet/freescale/dpaa2/dpaa2-eth-ceetm.c | 1219 ++++++++++++++++++++
+ .../net/ethernet/freescale/dpaa2/dpaa2-eth-ceetm.h |  207 ++++
+ drivers/net/ethernet/freescale/dpaa2/dpaa2-eth.c   |   53 +-
+ drivers/net/ethernet/freescale/dpaa2/dpaa2-eth.h   |    7 +
+ 6 files changed, 1482 insertions(+), 12 deletions(-)
+ create mode 100644 drivers/net/ethernet/freescale/dpaa2/dpaa2-eth-ceetm.c
+ create mode 100644 drivers/net/ethernet/freescale/dpaa2/dpaa2-eth-ceetm.h
+
+--- a/drivers/net/ethernet/freescale/dpaa2/Kconfig
++++ b/drivers/net/ethernet/freescale/dpaa2/Kconfig
+@@ -25,6 +25,13 @@ config FSL_DPAA2_ETH_USE_ERR_QUEUE
+ 	  in hardware).
+ 	  This may impact performance, recommended for debugging
+ 	  purposes only.
++
++config FSL_DPAA2_ETH_CEETM
++	depends on NET_SCHED
++	bool "DPAA2 Ethernet CEETM QoS"
++	default n
++	help
++	  Enable QoS offloading support through the CEETM hardware block.
+ endif
+ 
+ config FSL_DPAA2_PTP_CLOCK
+--- a/drivers/net/ethernet/freescale/dpaa2/Makefile
++++ b/drivers/net/ethernet/freescale/dpaa2/Makefile
+@@ -8,6 +8,7 @@ obj-$(CONFIG_FSL_DPAA2_PTP_CLOCK)	+= fsl
+ 
+ fsl-dpaa2-eth-objs	:= dpaa2-eth.o dpaa2-ethtool.o dpni.o
+ fsl-dpaa2-eth-${CONFIG_DEBUG_FS} += dpaa2-eth-debugfs.o
++fsl-dpaa2-eth-${CONFIG_FSL_DPAA2_ETH_CEETM} += dpaa2-eth-ceetm.o
+ fsl-dpaa2-ptp-objs	:= dpaa2-ptp.o dprtc.o
+ 
+ # Needed by the tracing framework
+--- /dev/null
++++ b/drivers/net/ethernet/freescale/dpaa2/dpaa2-eth-ceetm.c
+@@ -0,0 +1,1219 @@
++/* Copyright 2017 NXP
++ *
++ * Redistribution and use in source and binary forms, with or without
++ * modification, are permitted provided that the following conditions are met:
++ *     * Redistributions of source code must retain the above copyright
++ *	 notice, this list of conditions and the following disclaimer.
++ *     * Redistributions in binary form must reproduce the above copyright
++ *	 notice, this list of conditions and the following disclaimer in the
++ *	 documentation and/or other materials provided with the distribution.
++ *     * Neither the name of Freescale Semiconductor nor the
++ *	 names of its contributors may be used to endorse or promote products
++ *	 derived from this software without specific prior written permission.
++ *
++ *
++ * ALTERNATIVELY, this software may be distributed under the terms of the
++ * GNU General Public License ("GPL") as published by the Free Software
++ * Foundation, either version 2 of that License or (at your option) any
++ * later version.
++ *
++ * THIS SOFTWARE IS PROVIDED BY Freescale Semiconductor ``AS IS'' AND ANY
++ * EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
++ * WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
++ * DISCLAIMED. IN NO EVENT SHALL Freescale Semiconductor BE LIABLE FOR ANY
++ * DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
++ * (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
++ * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
++ * ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
++ * (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
++ * SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
++ */
++
++#include <linux/init.h>
++#include <linux/module.h>
++
++#include "dpaa2-eth-ceetm.h"
++#include "dpaa2-eth.h"
++
++#define DPAA2_CEETM_DESCRIPTION "FSL DPAA2 CEETM qdisc"
++/* Conversion formula from userspace passed Bps to expected Mbit */
++#define dpaa2_eth_bps_to_mbit(rate) (rate >> 17)
++
++static const struct nla_policy dpaa2_ceetm_policy[DPAA2_CEETM_TCA_MAX] = {
++	[DPAA2_CEETM_TCA_COPT] = { .len = sizeof(struct dpaa2_ceetm_tc_copt) },
++	[DPAA2_CEETM_TCA_QOPS] = { .len = sizeof(struct dpaa2_ceetm_tc_qopt) },
++};
++
++struct Qdisc_ops dpaa2_ceetm_qdisc_ops;
++
++static inline int dpaa2_eth_set_ch_shaping(struct dpaa2_eth_priv *priv,
++					   struct dpni_tx_shaping_cfg *scfg,
++					   struct dpni_tx_shaping_cfg *ecfg,
++					   int coupled, int ch_id)
++{
++	int err = 0;
++
++	netdev_dbg(priv->net_dev, "%s: ch_id %d rate %d mbps\n", __func__,
++		   ch_id, scfg->rate_limit);
++	err = dpni_set_tx_shaping(priv->mc_io, 0, priv->mc_token, scfg,
++				  ecfg, coupled);
++	if (err)
++		netdev_err(priv->net_dev, "dpni_set_tx_shaping err\n");
++
++	return err;
++}
++
++static inline int dpaa2_eth_reset_ch_shaping(struct dpaa2_eth_priv *priv,
++					     int ch_id)
++{
++	struct dpni_tx_shaping_cfg cfg = { 0 };
++
++	return dpaa2_eth_set_ch_shaping(priv, &cfg, &cfg, 0, ch_id);
++}
++
++static inline int
++dpaa2_eth_update_shaping_cfg(struct net_device *dev,
++			     struct dpaa2_ceetm_shaping_cfg cfg,
++			     struct dpni_tx_shaping_cfg *scfg,
++			     struct dpni_tx_shaping_cfg *ecfg)
++{
++	scfg->rate_limit = dpaa2_eth_bps_to_mbit(cfg.cir);
++	ecfg->rate_limit = dpaa2_eth_bps_to_mbit(cfg.eir);
++
++	if (cfg.cbs > DPAA2_ETH_MAX_BURST_SIZE) {
++		netdev_err(dev, "Committed burst size must be under %d\n",
++			   DPAA2_ETH_MAX_BURST_SIZE);
++		return -EINVAL;
++	}
++
++	scfg->max_burst_size = cfg.cbs;
++
++	if (cfg.ebs > DPAA2_ETH_MAX_BURST_SIZE) {
++		netdev_err(dev, "Excess burst size must be under %d\n",
++			   DPAA2_ETH_MAX_BURST_SIZE);
++		return -EINVAL;
++	}
++
++	ecfg->max_burst_size = cfg.ebs;
++
++	if ((!cfg.cir || !cfg.eir) && cfg.coupled) {
++		netdev_err(dev, "Coupling can be set when both CIR and EIR are finite\n");
++		return -EINVAL;
++	}
++
++	return 0;
++}
++
++enum update_tx_prio {
++	DPAA2_ETH_ADD_CQ,
++	DPAA2_ETH_DEL_CQ,
++};
++
++/* Normalize weights based on max passed value */
++static inline int dpaa2_eth_normalize_tx_prio(struct dpaa2_ceetm_qdisc *priv)
++{
++	struct dpni_tx_schedule_cfg *sched_cfg;
++	struct dpaa2_ceetm_class *cl;
++	u32 qpri;
++	u16 weight_max = 0, increment;
++	int i;
++
++	/* Check the boundaries of the provided values */
++	for (i = 0; i < priv->clhash.hashsize; i++)
++		hlist_for_each_entry(cl, &priv->clhash.hash[i], common.hnode)
++			weight_max = (weight_max == 0 ? cl->prio.weight :
++				     (weight_max < cl->prio.weight ?
++				      cl->prio.weight : weight_max));
++
++	/* If there are no elements, there's nothing to do */
++	if (weight_max == 0)
++		return 0;
++
++	increment = (DPAA2_CEETM_MAX_WEIGHT - DPAA2_CEETM_MIN_WEIGHT) /
++		    weight_max;
++
++	for (i = 0; i < priv->clhash.hashsize; i++) {
++		hlist_for_each_entry(cl, &priv->clhash.hash[i], common.hnode) {
++			if (cl->prio.mode == STRICT_PRIORITY)
++				continue;
++
++			qpri = cl->prio.qpri;
++			sched_cfg = &priv->prio.tx_prio_cfg.tc_sched[qpri];
++
++			sched_cfg->delta_bandwidth =
++				DPAA2_CEETM_MIN_WEIGHT +
++				(cl->prio.weight * increment);
++
++			pr_debug("%s: Normalized CQ qpri %d weight to %d\n",
++				 __func__, qpri, sched_cfg->delta_bandwidth);
++		}
++	}
++
++	return 0;
++}
++
++static inline int dpaa2_eth_update_tx_prio(struct dpaa2_eth_priv *priv,
++					   struct dpaa2_ceetm_class *cl,
++					   enum update_tx_prio type)
++{
++	struct dpaa2_ceetm_qdisc *sch = qdisc_priv(cl->parent);
++	struct dpni_tx_schedule_cfg *sched_cfg;
++	struct dpni_taildrop td = {0};
++	u8 ch_id = 0, tc_id = 0;
++	u32 qpri = 0;
++	int err = 0;
++
++	qpri = cl->prio.qpri;
++	tc_id = DPNI_BUILD_CH_TC(ch_id, qpri);
++
++	switch (type) {
++	case DPAA2_ETH_ADD_CQ:
++		/* Enable taildrop */
++		td.enable = 1;
++		td.units = DPNI_CONGESTION_UNIT_FRAMES;
++		td.threshold = DPAA2_CEETM_TD_THRESHOLD;
++		err = dpni_set_taildrop(priv->mc_io, 0, priv->mc_token,
++					DPNI_CP_GROUP, DPNI_QUEUE_TX, tc_id,
++					0, &td);
++		if (err) {
++			netdev_err(priv->net_dev, "Error enabling Tx taildrop %d\n",
++				   err);
++			return err;
++		}
++		break;
++	case DPAA2_ETH_DEL_CQ:
++		/* Disable taildrop */
++		td.enable = 0;
++		err = dpni_set_taildrop(priv->mc_io, 0, priv->mc_token,
++					DPNI_CP_GROUP, DPNI_QUEUE_TX, tc_id,
++					0, &td);
++		if (err) {
++			netdev_err(priv->net_dev, "Error disabling Tx taildrop %d\n",
++				   err);
++			return err;
++		}
++		break;
++	}
++
++	/* We can zero out the structure in the tx_prio_conf array */
++	if (type == DPAA2_ETH_DEL_CQ) {
++		sched_cfg = &sch->prio.tx_prio_cfg.tc_sched[qpri];
++		memset(sched_cfg, 0, sizeof(*sched_cfg));
++	}
++
++	/* Normalize priorities */
++	err = dpaa2_eth_normalize_tx_prio(sch);
++
++	/* Debug print goes here */
++	print_hex_dump_debug("tx_prio: ", DUMP_PREFIX_OFFSET, 16, 1,
++			     &sch->prio.tx_prio_cfg,
++			     sizeof(sch->prio.tx_prio_cfg), 0);
++
++	/* Call dpni_set_tx_priorities for the entire prio qdisc */
++	err = dpni_set_tx_priorities(priv->mc_io, 0, priv->mc_token,
++				     &sch->prio.tx_prio_cfg);
++	if (err)
++		netdev_err(priv->net_dev, "dpni_set_tx_priorities err %d\n",
++			   err);
++
++	return err;
++}
++
++static void dpaa2_eth_ceetm_enable(struct dpaa2_eth_priv *priv)
++{
++	priv->ceetm_en = true;
++}
++
++static void dpaa2_eth_ceetm_disable(struct dpaa2_eth_priv *priv)
++{
++	priv->ceetm_en = false;
++}
++
++/* Find class in qdisc hash table using given handle */
++static inline struct dpaa2_ceetm_class *dpaa2_ceetm_find(u32 handle,
++							 struct Qdisc *sch)
++{
++	struct dpaa2_ceetm_qdisc *priv = qdisc_priv(sch);
++	struct Qdisc_class_common *clc;
++
++	pr_debug(KBUILD_BASENAME " : %s : find class %X in qdisc %X\n",
++		 __func__, handle, sch->handle);
++
++	clc = qdisc_class_find(&priv->clhash, handle);
++	return clc ? container_of(clc, struct dpaa2_ceetm_class, common) : NULL;
++}
++
++/* Insert a class in the qdisc's class hash */
++static void dpaa2_ceetm_link_class(struct Qdisc *sch,
++				   struct Qdisc_class_hash *clhash,
++				   struct Qdisc_class_common *common)
++{
++	sch_tree_lock(sch);
++	qdisc_class_hash_insert(clhash, common);
++	sch_tree_unlock(sch);
++	qdisc_class_hash_grow(sch, clhash);
++}
++
++/* Destroy a ceetm class */
++static void dpaa2_ceetm_cls_destroy(struct Qdisc *sch,
++				    struct dpaa2_ceetm_class *cl)
++{
++	struct net_device *dev = qdisc_dev(sch);
++	struct dpaa2_eth_priv *priv = netdev_priv(dev);
++
++	if (!cl)
++		return;
++
++	pr_debug(KBUILD_BASENAME " : %s : destroy class %X from under %X\n",
++		 __func__, cl->common.classid, sch->handle);
++
++	/* Recurse into child first */
++	if (cl->child) {
++		qdisc_put(cl->child);
++		cl->child = NULL;
++	}
++
++	switch (cl->type) {
++	case CEETM_ROOT:
++		if (dpaa2_eth_reset_ch_shaping(priv, cl->root.ch_id))
++			netdev_err(dev, "Error resetting channel shaping\n");
++
++		break;
++
++	case CEETM_PRIO:
++		if (dpaa2_eth_update_tx_prio(priv, cl, DPAA2_ETH_DEL_CQ))
++			netdev_err(dev, "Error resetting tx_priorities\n");
++
++		if (cl->prio.cstats)
++			free_percpu(cl->prio.cstats);
++
++		break;
++	}
++
++	tcf_block_put(cl->block);
++	kfree(cl);
++}
++
++/* Destroy a ceetm qdisc */
++static void dpaa2_ceetm_destroy(struct Qdisc *sch)
++{
++	unsigned int i;
++	struct hlist_node *next;
++	struct dpaa2_ceetm_class *cl;
++	struct dpaa2_ceetm_qdisc *priv = qdisc_priv(sch);
++	struct net_device *dev = qdisc_dev(sch);
++	struct dpaa2_eth_priv *priv_eth = netdev_priv(dev);
++
++	pr_debug(KBUILD_BASENAME " : %s : destroy qdisc %X\n",
++		 __func__, sch->handle);
++
++	/* All filters need to be removed before destroying the classes */
++	tcf_block_put(priv->block);
++
++	for (i = 0; i < priv->clhash.hashsize; i++) {
++		hlist_for_each_entry(cl, &priv->clhash.hash[i], common.hnode)
++			tcf_block_put(cl->block);
++	}
++
++	for (i = 0; i < priv->clhash.hashsize; i++) {
++		hlist_for_each_entry_safe(cl, next, &priv->clhash.hash[i],
++					  common.hnode)
++			dpaa2_ceetm_cls_destroy(sch, cl);
++	}
++
++	qdisc_class_hash_destroy(&priv->clhash);
++
++	switch (priv->type) {
++	case CEETM_ROOT:
++		dpaa2_eth_ceetm_disable(priv_eth);
++
++		if (priv->root.qstats)
++			free_percpu(priv->root.qstats);
++
++		if (!priv->root.qdiscs)
++			break;
++
++		/* Destroy the pfifo qdiscs in case they haven't been attached
++		 * to the netdev queues yet.
++		 */
++		for (i = 0; i < dev->num_tx_queues; i++)
++			if (priv->root.qdiscs[i])
++				qdisc_put(priv->root.qdiscs[i]);
++
++		kfree(priv->root.qdiscs);
++		break;
++
++	case CEETM_PRIO:
++		if (priv->prio.parent)
++			priv->prio.parent->child = NULL;
++		break;
++	}
++}
++
++static int dpaa2_ceetm_dump(struct Qdisc *sch, struct sk_buff *skb)
++{
++	struct Qdisc *qdisc;
++	unsigned int ntx, i;
++	struct nlattr *nest;
++	struct dpaa2_ceetm_tc_qopt qopt;
++	struct dpaa2_ceetm_qdisc_stats *qstats;
++	struct net_device *dev = qdisc_dev(sch);
++	struct dpaa2_ceetm_qdisc *priv = qdisc_priv(sch);
++
++	pr_debug(KBUILD_BASENAME " : %s : qdisc %X\n", __func__, sch->handle);
++
++	sch_tree_lock(sch);
++	memset(&qopt, 0, sizeof(qopt));
++	qopt.type = priv->type;
++	qopt.shaped = priv->shaped;
++
++	switch (priv->type) {
++	case CEETM_ROOT:
++		/* Gather statistics from the underlying pfifo qdiscs */
++		sch->q.qlen = 0;
++		memset(&sch->bstats, 0, sizeof(sch->bstats));
++		memset(&sch->qstats, 0, sizeof(sch->qstats));
++
++		for (ntx = 0; ntx < dev->num_tx_queues; ntx++) {
++			qdisc = netdev_get_tx_queue(dev, ntx)->qdisc_sleeping;
++			sch->q.qlen		+= qdisc->q.qlen;
++			sch->bstats.bytes	+= qdisc->bstats.bytes;
++			sch->bstats.packets	+= qdisc->bstats.packets;
++			sch->qstats.qlen	+= qdisc->qstats.qlen;
++			sch->qstats.backlog	+= qdisc->qstats.backlog;
++			sch->qstats.drops	+= qdisc->qstats.drops;
++			sch->qstats.requeues	+= qdisc->qstats.requeues;
++			sch->qstats.overlimits	+= qdisc->qstats.overlimits;
++		}
++
++		for_each_online_cpu(i) {
++			qstats = per_cpu_ptr(priv->root.qstats, i);
++			sch->qstats.drops += qstats->drops;
++		}
++
++		break;
++
++	case CEETM_PRIO:
++		qopt.prio_group_A = priv->prio.tx_prio_cfg.prio_group_A;
++		qopt.prio_group_B = priv->prio.tx_prio_cfg.prio_group_B;
++		qopt.separate_groups = priv->prio.tx_prio_cfg.separate_groups;
++		break;
++
++	default:
++		pr_err(KBUILD_BASENAME " : %s : invalid qdisc\n", __func__);
++		sch_tree_unlock(sch);
++		return -EINVAL;
++	}
++
++	nest = nla_nest_start_noflag(skb, TCA_OPTIONS);
++	if (!nest)
++		goto nla_put_failure;
++	if (nla_put(skb, DPAA2_CEETM_TCA_QOPS, sizeof(qopt), &qopt))
++		goto nla_put_failure;
++	nla_nest_end(skb, nest);
++
++	sch_tree_unlock(sch);
++	return skb->len;
++
++nla_put_failure:
++	sch_tree_unlock(sch);
++	nla_nest_cancel(skb, nest);
++	return -EMSGSIZE;
++}
++
++static int dpaa2_ceetm_change_prio(struct Qdisc *sch,
++				   struct dpaa2_ceetm_qdisc *priv,
++				   struct dpaa2_ceetm_tc_qopt *qopt)
++{
++	/* TODO: Once LX2 support is added */
++	/* priv->shaped = parent_cl->shaped; */
++	priv->prio.tx_prio_cfg.prio_group_A = qopt->prio_group_A;
++	priv->prio.tx_prio_cfg.prio_group_B = qopt->prio_group_B;
++	priv->prio.tx_prio_cfg.separate_groups = qopt->separate_groups;
++
++	return 0;
++}
++
++/* Edit a ceetm qdisc */
++static int dpaa2_ceetm_change(struct Qdisc *sch, struct nlattr *opt,
++			      struct netlink_ext_ack *extack)
++{
++	struct dpaa2_ceetm_qdisc *priv = qdisc_priv(sch);
++	struct nlattr *tb[DPAA2_CEETM_TCA_QOPS + 1];
++	struct dpaa2_ceetm_tc_qopt *qopt;
++	int err;
++
++	pr_debug(KBUILD_BASENAME " : %s : qdisc %X\n", __func__, sch->handle);
++
++	err = nla_parse_nested_deprecated(tb, DPAA2_CEETM_TCA_QOPS, opt,
++					  dpaa2_ceetm_policy, extack);
++	if (err < 0) {
++		pr_err(KBUILD_BASENAME " : %s : tc error in %s\n", __func__,
++		       "nla_parse_nested_deprecated");
++		return err;
++	}
++
++	if (!tb[DPAA2_CEETM_TCA_QOPS]) {
++		pr_err(KBUILD_BASENAME " : %s : tc error in %s\n", __func__,
++		       "tb");
++		return -EINVAL;
++	}
++
++	if (TC_H_MIN(sch->handle)) {
++		pr_err("CEETM: a qdisc should not have a minor\n");
++		return -EINVAL;
++	}
++
++	qopt = nla_data(tb[DPAA2_CEETM_TCA_QOPS]);
++
++	if (priv->type != qopt->type) {
++		pr_err("CEETM: qdisc %X is not of the provided type\n",
++		       sch->handle);
++		return -EINVAL;
++	}
++
++	switch (priv->type) {
++	case CEETM_PRIO:
++		err = dpaa2_ceetm_change_prio(sch, priv, qopt);
++		break;
++	default:
++		pr_err(KBUILD_BASENAME " : %s : invalid qdisc\n", __func__);
++		err = -EINVAL;
++	}
++
++	return err;
++}
++
++/* Configure a root ceetm qdisc */
++static int dpaa2_ceetm_init_root(struct Qdisc *sch,
++				 struct dpaa2_ceetm_qdisc *priv,
++				 struct dpaa2_ceetm_tc_qopt *qopt,
++				 struct netlink_ext_ack *extack)
++{
++	struct net_device *dev = qdisc_dev(sch);
++	struct dpaa2_eth_priv *priv_eth = netdev_priv(dev);
++	struct netdev_queue *dev_queue;
++	unsigned int i, parent_id;
++	struct Qdisc *qdisc;
++
++	pr_debug(KBUILD_BASENAME " : %s : qdisc %X\n", __func__, sch->handle);
++
++	/* Validate inputs */
++	if (sch->parent != TC_H_ROOT) {
++		pr_err("CEETM: a root ceetm qdisc must be root\n");
++		return -EINVAL;
++	}
++
++	/* Pre-allocate underlying pfifo qdiscs.
++	 *
++	 * We want to offload shaping and scheduling decisions to the hardware.
++	 * The pfifo qdiscs will be attached to the netdev queues and will
++	 * guide the traffic from the IP stack down to the driver with minimum
++	 * interference.
++	 *
++	 * The CEETM qdiscs and classes will be crossed when the traffic
++	 * reaches the driver.
++	 */
++	priv->root.qdiscs = kcalloc(dev->num_tx_queues,
++				    sizeof(priv->root.qdiscs[0]),
++				    GFP_KERNEL);
++	if (!priv->root.qdiscs)
++		return -ENOMEM;
++
++	for (i = 0; i < dev->num_tx_queues; i++) {
++		dev_queue = netdev_get_tx_queue(dev, i);
++		parent_id = TC_H_MAKE(TC_H_MAJ(sch->handle),
++				      TC_H_MIN(i + PFIFO_MIN_OFFSET));
++
++		qdisc = qdisc_create_dflt(dev_queue, &pfifo_qdisc_ops,
++					  parent_id, extack);
++		if (!qdisc)
++			return -ENOMEM;
++
++		priv->root.qdiscs[i] = qdisc;
++		qdisc->flags |= TCQ_F_ONETXQUEUE;
++	}
++
++	sch->flags |= TCQ_F_MQROOT;
++
++	priv->root.qstats = alloc_percpu(struct dpaa2_ceetm_qdisc_stats);
++	if (!priv->root.qstats) {
++		pr_err(KBUILD_BASENAME " : %s : alloc_percpu() failed\n",
++		       __func__);
++		return -ENOMEM;
++	}
++
++	dpaa2_eth_ceetm_enable(priv_eth);
++	return 0;
++}
++
++/* Configure a prio ceetm qdisc */
++static int dpaa2_ceetm_init_prio(struct Qdisc *sch,
++				 struct dpaa2_ceetm_qdisc *priv,
++				 struct dpaa2_ceetm_tc_qopt *qopt)
++{
++	struct net_device *dev = qdisc_dev(sch);
++	struct dpaa2_ceetm_class *parent_cl;
++	struct Qdisc *parent_qdisc;
++
++	pr_debug(KBUILD_BASENAME " : %s : qdisc %X\n", __func__, sch->handle);
++
++	if (sch->parent == TC_H_ROOT) {
++		pr_err("CEETM: a prio ceetm qdisc can not be root\n");
++		return -EINVAL;
++	}
++
++	parent_qdisc = qdisc_lookup(dev, TC_H_MAJ(sch->parent));
++	if (strcmp(parent_qdisc->ops->id, dpaa2_ceetm_qdisc_ops.id)) {
++		pr_err("CEETM: a ceetm qdisc can not be attached to other qdisc/class types\n");
++		return -EINVAL;
++	}
++
++	/* Obtain the parent root ceetm_class */
++	parent_cl = dpaa2_ceetm_find(sch->parent, parent_qdisc);
++
++	if (!parent_cl || parent_cl->type != CEETM_ROOT) {
++		pr_err("CEETM: a prio ceetm qdiscs can be added only under a root ceetm class\n");
++		return -EINVAL;
++	}
++
++	priv->prio.parent = parent_cl;
++	parent_cl->child = sch;
++
++	return dpaa2_ceetm_change_prio(sch, priv, qopt);
++}
++
++/* Configure a generic ceetm qdisc */
++static int dpaa2_ceetm_init(struct Qdisc *sch, struct nlattr *opt,
++			    struct netlink_ext_ack *extack)
++{
++	struct dpaa2_ceetm_qdisc *priv = qdisc_priv(sch);
++	struct net_device *dev = qdisc_dev(sch);
++	struct nlattr *tb[DPAA2_CEETM_TCA_QOPS + 1];
++	struct dpaa2_ceetm_tc_qopt *qopt;
++	int err;
++
++	pr_debug(KBUILD_BASENAME " : %s : qdisc %X\n", __func__, sch->handle);
++
++	if (!netif_is_multiqueue(dev))
++		return -EOPNOTSUPP;
++
++	err = tcf_block_get(&priv->block, &priv->filter_list, sch, extack);
++	if (err) {
++		pr_err("CEETM: unable to get tcf_block\n");
++		return err;
++	}
++
++	if (!opt) {
++		pr_err(KBUILD_BASENAME " : %s : tc error - opt = NULL\n",
++		       __func__);
++		return -EINVAL;
++	}
++
++	err = nla_parse_nested_deprecated(tb, DPAA2_CEETM_TCA_QOPS, opt,
++					  dpaa2_ceetm_policy, extack);
++	if (err < 0) {
++		pr_err(KBUILD_BASENAME " : %s : tc error in %s\n", __func__,
++		       "nla_parse_nested_deprecated");
++		return err;
++	}
++
++	if (!tb[DPAA2_CEETM_TCA_QOPS]) {
++		pr_err(KBUILD_BASENAME " : %s : tc error in %s\n", __func__,
++		       "tb");
++		return -EINVAL;
++	}
++
++	if (TC_H_MIN(sch->handle)) {
++		pr_err("CEETM: a qdisc should not have a minor\n");
++		return -EINVAL;
++	}
++
++	qopt = nla_data(tb[DPAA2_CEETM_TCA_QOPS]);
++
++	/* Initialize the class hash list. Each qdisc has its own class hash */
++	err = qdisc_class_hash_init(&priv->clhash);
++	if (err < 0) {
++		pr_err(KBUILD_BASENAME " : %s : qdisc_class_hash_init failed\n",
++		       __func__);
++		return err;
++	}
++
++	priv->type = qopt->type;
++	priv->shaped = qopt->shaped;
++
++	switch (priv->type) {
++	case CEETM_ROOT:
++		err = dpaa2_ceetm_init_root(sch, priv, qopt, extack);
++		break;
++	case CEETM_PRIO:
++		err = dpaa2_ceetm_init_prio(sch, priv, qopt);
++		break;
++	default:
++		pr_err(KBUILD_BASENAME " : %s : invalid qdisc\n", __func__);
++		/* Note: dpaa2_ceetm_destroy() will be called by our caller */
++		err = -EINVAL;
++	}
++
++	return err;
++}
++
++/* Attach the underlying pfifo qdiscs */
++static void dpaa2_ceetm_attach(struct Qdisc *sch)
++{
++	struct net_device *dev = qdisc_dev(sch);
++	struct dpaa2_ceetm_qdisc *priv = qdisc_priv(sch);
++	struct Qdisc *qdisc, *old_qdisc;
++	unsigned int i;
++
++	pr_debug(KBUILD_BASENAME " : %s : qdisc %X\n", __func__, sch->handle);
++
++	for (i = 0; i < dev->num_tx_queues; i++) {
++		qdisc = priv->root.qdiscs[i];
++		old_qdisc = dev_graft_qdisc(qdisc->dev_queue, qdisc);
++		if (old_qdisc)
++			qdisc_put(old_qdisc);
++	}
++
++	/* Remove the references to the pfifo qdiscs since the kernel will
++	 * destroy them when needed. No cleanup from our part is required from
++	 * this point on.
++	 */
++	kfree(priv->root.qdiscs);
++	priv->root.qdiscs = NULL;
++}
++
++static unsigned long dpaa2_ceetm_cls_find(struct Qdisc *sch, u32 classid)
++{
++	struct dpaa2_ceetm_class *cl;
++
++	pr_debug(KBUILD_BASENAME " : %s : classid %X from qdisc %X\n",
++		 __func__, classid, sch->handle);
++	cl = dpaa2_ceetm_find(classid, sch);
++
++	return (unsigned long)cl;
++}
++
++static int dpaa2_ceetm_cls_change_root(struct dpaa2_ceetm_class *cl,
++				       struct dpaa2_ceetm_tc_copt *copt,
++				       struct net_device *dev)
++{
++	struct dpaa2_eth_priv *priv = netdev_priv(dev);
++	struct dpni_tx_shaping_cfg scfg = { 0 }, ecfg = { 0 };
++	int err = 0;
++
++	pr_debug(KBUILD_BASENAME " : %s : class %X\n", __func__,
++		 cl->common.classid);
++
++	if (!cl->shaped)
++		return 0;
++
++	if (dpaa2_eth_update_shaping_cfg(dev, copt->shaping_cfg,
++					 &scfg, &ecfg))
++		return -EINVAL;
++
++	err = dpaa2_eth_set_ch_shaping(priv, &scfg, &ecfg,
++				       copt->shaping_cfg.coupled,
++				       cl->root.ch_id);
++	if (err)
++		return err;
++
++	memcpy(&cl->root.shaping_cfg, &copt->shaping_cfg,
++	       sizeof(struct dpaa2_ceetm_shaping_cfg));
++
++	return err;
++}
++
++static int dpaa2_ceetm_cls_change_prio(struct dpaa2_ceetm_class *cl,
++				       struct dpaa2_ceetm_tc_copt *copt,
++				       struct net_device *dev)
++{
++	struct dpaa2_ceetm_qdisc *sch = qdisc_priv(cl->parent);
++	struct dpni_tx_schedule_cfg *sched_cfg;
++	struct dpaa2_eth_priv *priv = netdev_priv(dev);
++	int err;
++
++	pr_debug(KBUILD_BASENAME " : %s : class %X mode %d weight %d\n",
++		 __func__, cl->common.classid, copt->mode, copt->weight);
++
++	if (!cl->prio.cstats) {
++		cl->prio.cstats = alloc_percpu(struct dpaa2_ceetm_class_stats);
++		if (!cl->prio.cstats) {
++			pr_err(KBUILD_BASENAME " : %s : alloc_percpu() failed\n",
++			       __func__);
++			return -ENOMEM;
++		}
++	}
++
++	cl->prio.mode = copt->mode;
++	cl->prio.weight = copt->weight;
++
++	sched_cfg = &sch->prio.tx_prio_cfg.tc_sched[cl->prio.qpri];
++
++	switch (copt->mode) {
++	case STRICT_PRIORITY:
++		sched_cfg->mode = DPNI_TX_SCHED_STRICT_PRIORITY;
++		break;
++	case WEIGHTED_A:
++		sched_cfg->mode = DPNI_TX_SCHED_WEIGHTED_A;
++		break;
++	case WEIGHTED_B:
++		sched_cfg->mode = DPNI_TX_SCHED_WEIGHTED_B;
++		break;
++	}
++
++	err = dpaa2_eth_update_tx_prio(priv, cl, DPAA2_ETH_ADD_CQ);
++
++	return err;
++}
++
++/* Add a new ceetm class */
++static int dpaa2_ceetm_cls_add(struct Qdisc *sch, u32 classid,
++			       struct dpaa2_ceetm_tc_copt *copt,
++			       unsigned long *arg,
++			       struct netlink_ext_ack *extack)
++{
++	struct dpaa2_ceetm_qdisc *priv = qdisc_priv(sch);
++	struct net_device *dev = qdisc_dev(sch);
++	struct dpaa2_eth_priv *priv_eth = netdev_priv(dev);
++	struct dpaa2_ceetm_class *cl;
++	int err;
++
++	if (copt->type == CEETM_ROOT &&
++	    priv->clhash.hashelems == dpaa2_eth_ch_count(priv_eth)) {
++		pr_err("CEETM: only %d channel%s per DPNI allowed, sorry\n",
++		       dpaa2_eth_ch_count(priv_eth),
++		       dpaa2_eth_ch_count(priv_eth) == 1 ? "" : "s");
++		return -EINVAL;
++	}
++
++	if (copt->type == CEETM_PRIO &&
++	    priv->clhash.hashelems == dpaa2_eth_tc_count(priv_eth)) {
++		pr_err("CEETM: only %d queue%s per channel allowed, sorry\n",
++		       dpaa2_eth_tc_count(priv_eth),
++		       dpaa2_eth_tc_count(priv_eth) == 1 ? "" : "s");
++		return -EINVAL;
++	}
++
++	cl = kzalloc(sizeof(*cl), GFP_KERNEL);
++	if (!cl)
++		return -ENOMEM;
++
++	err = tcf_block_get(&cl->block, &cl->filter_list, sch, extack);
++	if (err) {
++		pr_err("%s: Unable to set new root class\n", __func__);
++		goto out_free;
++	}
++
++	cl->common.classid = classid;
++	cl->parent = sch;
++	cl->child = NULL;
++
++	/* Add class handle in Qdisc */
++	dpaa2_ceetm_link_class(sch, &priv->clhash, &cl->common);
++
++	cl->shaped = copt->shaped;
++	cl->type = copt->type;
++
++	/* Claim a CEETM channel / tc - DPAA2. will assume transition from
++	 * classid to qdid/qpri, starting from qdid / qpri 0
++	 */
++	switch (copt->type) {
++	case CEETM_ROOT:
++		cl->root.ch_id = classid - sch->handle - 1;
++		err = dpaa2_ceetm_cls_change_root(cl, copt, dev);
++		break;
++	case CEETM_PRIO:
++		cl->prio.qpri = classid - sch->handle - 1;
++		err = dpaa2_ceetm_cls_change_prio(cl, copt, dev);
++		break;
++	}
++
++	if (err) {
++		pr_err("%s: Unable to set new %s class\n", __func__,
++		       (copt->type == CEETM_ROOT ? "root" : "prio"));
++		goto out_free;
++	}
++
++	switch (copt->type) {
++	case CEETM_ROOT:
++		pr_debug(KBUILD_BASENAME " : %s : configured root class %X associated with channel qdid %d\n",
++			 __func__, classid, cl->root.ch_id);
++		break;
++	case CEETM_PRIO:
++		pr_debug(KBUILD_BASENAME " : %s : configured prio class %X associated with queue qpri %d\n",
++			 __func__, classid, cl->prio.qpri);
++		break;
++	}
++
++	*arg = (unsigned long)cl;
++	return 0;
++
++out_free:
++	kfree(cl);
++	return err;
++}
++
++/* Add or configure a ceetm class */
++static int dpaa2_ceetm_cls_change(struct Qdisc *sch, u32 classid, u32 parentid,
++				  struct nlattr **tca, unsigned long *arg,
++				  struct netlink_ext_ack *extack)
++{
++	struct dpaa2_ceetm_qdisc *priv;
++	struct dpaa2_ceetm_class *cl = (struct dpaa2_ceetm_class *)*arg;
++	struct nlattr *opt = tca[TCA_OPTIONS];
++	struct nlattr *tb[DPAA2_CEETM_TCA_MAX];
++	struct dpaa2_ceetm_tc_copt *copt;
++	struct net_device *dev = qdisc_dev(sch);
++	int err;
++
++	pr_debug(KBUILD_BASENAME " : %s : classid %X under qdisc %X\n",
++		 __func__, classid, sch->handle);
++
++	if (strcmp(sch->ops->id, dpaa2_ceetm_qdisc_ops.id)) {
++		pr_err("CEETM: a ceetm class can not be attached to other qdisc/class types\n");
++		return -EINVAL;
++	}
++
++	priv = qdisc_priv(sch);
++
++	if (!opt) {
++		pr_err(KBUILD_BASENAME " : %s : tc error NULL opt\n", __func__);
++		return -EINVAL;
++	}
++
++	err = nla_parse_nested_deprecated(tb, DPAA2_CEETM_TCA_COPT, opt,
++					  dpaa2_ceetm_policy, extack);
++	if (err < 0) {
++		pr_err(KBUILD_BASENAME " : %s : tc error in %s\n", __func__,
++		       "nla_parse_nested_deprecated");
++		return -EINVAL;
++	}
++
++	if (!tb[DPAA2_CEETM_TCA_COPT]) {
++		pr_err(KBUILD_BASENAME " : %s : tc error in %s\n", __func__,
++		       "tb");
++		return -EINVAL;
++	}
++
++	copt = nla_data(tb[DPAA2_CEETM_TCA_COPT]);
++
++	/* Configure an existing ceetm class */
++	if (cl) {
++		if (copt->type != cl->type) {
++			pr_err("CEETM: class %X is not of the provided type\n",
++			       cl->common.classid);
++			return -EINVAL;
++		}
++
++		switch (copt->type) {
++		case CEETM_ROOT:
++			return dpaa2_ceetm_cls_change_root(cl, copt, dev);
++		case CEETM_PRIO:
++			return dpaa2_ceetm_cls_change_prio(cl, copt, dev);
++
++		default:
++			pr_err(KBUILD_BASENAME " : %s : invalid class\n",
++			       __func__);
++			return -EINVAL;
++		}
++	}
++
++	return dpaa2_ceetm_cls_add(sch, classid, copt, arg, extack);
++}
++
++static void dpaa2_ceetm_cls_walk(struct Qdisc *sch, struct qdisc_walker *arg)
++{
++	struct dpaa2_ceetm_qdisc *priv = qdisc_priv(sch);
++	struct dpaa2_ceetm_class *cl;
++	unsigned int i;
++
++	pr_debug(KBUILD_BASENAME " : %s : qdisc %X\n", __func__, sch->handle);
++
++	if (arg->stop)
++		return;
++
++	for (i = 0; i < priv->clhash.hashsize; i++) {
++		hlist_for_each_entry(cl, &priv->clhash.hash[i], common.hnode) {
++			if (arg->count < arg->skip) {
++				arg->count++;
++				continue;
++			}
++			if (arg->fn(sch, (unsigned long)cl, arg) < 0) {
++				arg->stop = 1;
++				return;
++			}
++			arg->count++;
++		}
++	}
++}
++
++static int dpaa2_ceetm_cls_dump(struct Qdisc *sch, unsigned long arg,
++				struct sk_buff *skb, struct tcmsg *tcm)
++{
++	struct dpaa2_ceetm_class *cl = (struct dpaa2_ceetm_class *)arg;
++	struct nlattr *nest;
++	struct dpaa2_ceetm_tc_copt copt;
++
++	pr_debug(KBUILD_BASENAME " : %s : class %X under qdisc %X\n",
++		 __func__, cl->common.classid, sch->handle);
++
++	sch_tree_lock(sch);
++
++	tcm->tcm_parent = ((struct Qdisc *)cl->parent)->handle;
++	tcm->tcm_handle = cl->common.classid;
++
++	memset(&copt, 0, sizeof(copt));
++
++	copt.shaped = cl->shaped;
++	copt.type = cl->type;
++
++	switch (cl->type) {
++	case CEETM_ROOT:
++		if (cl->child)
++			tcm->tcm_info = cl->child->handle;
++
++		memcpy(&copt.shaping_cfg, &cl->root.shaping_cfg,
++		       sizeof(struct dpaa2_ceetm_shaping_cfg));
++
++		break;
++
++	case CEETM_PRIO:
++		if (cl->child)
++			tcm->tcm_info = cl->child->handle;
++
++		copt.mode = cl->prio.mode;
++		copt.weight = cl->prio.weight;
++
++		break;
++	}
++
++	nest = nla_nest_start_noflag(skb, TCA_OPTIONS);
++	if (!nest)
++		goto nla_put_failure;
++	if (nla_put(skb, DPAA2_CEETM_TCA_COPT, sizeof(copt), &copt))
++		goto nla_put_failure;
++	nla_nest_end(skb, nest);
++	sch_tree_unlock(sch);
++	return skb->len;
++
++nla_put_failure:
++	sch_tree_unlock(sch);
++	nla_nest_cancel(skb, nest);
++	return -EMSGSIZE;
++}
++
++static int dpaa2_ceetm_cls_delete(struct Qdisc *sch, unsigned long arg)
++{
++	struct dpaa2_ceetm_qdisc *priv = qdisc_priv(sch);
++	struct dpaa2_ceetm_class *cl = (struct dpaa2_ceetm_class *)arg;
++
++	pr_debug(KBUILD_BASENAME " : %s : class %X under qdisc %X\n",
++		 __func__, cl->common.classid, sch->handle);
++
++	sch_tree_lock(sch);
++	qdisc_class_hash_remove(&priv->clhash, &cl->common);
++	sch_tree_unlock(sch);
++	return 0;
++}
++
++/* Get the class' child qdisc, if any */
++static struct Qdisc *dpaa2_ceetm_cls_leaf(struct Qdisc *sch, unsigned long arg)
++{
++	struct dpaa2_ceetm_class *cl = (struct dpaa2_ceetm_class *)arg;
++
++	pr_debug(KBUILD_BASENAME " : %s : class %X under qdisc %X\n",
++		 __func__, cl->common.classid, sch->handle);
++
++	switch (cl->type) {
++	case CEETM_ROOT:
++	case CEETM_PRIO:
++		return cl->child;
++	}
++
++	return NULL;
++}
++
++static int dpaa2_ceetm_cls_graft(struct Qdisc *sch, unsigned long arg,
++				 struct Qdisc *new, struct Qdisc **old,
++				 struct netlink_ext_ack *extack)
++{
++	if (new && strcmp(new->ops->id, dpaa2_ceetm_qdisc_ops.id)) {
++		pr_err("CEETM: only ceetm qdiscs can be attached to ceetm classes\n");
++		return -EOPNOTSUPP;
++	}
++
++	return 0;
++}
++
++static int dpaa2_ceetm_cls_dump_stats(struct Qdisc *sch, unsigned long arg,
++				      struct gnet_dump *d)
++{
++	struct dpaa2_ceetm_class *cl = (struct dpaa2_ceetm_class *)arg;
++	struct gnet_stats_basic_packed tmp_bstats;
++	struct dpaa2_ceetm_tc_xstats xstats;
++	union dpni_statistics dpni_stats;
++	struct net_device *dev = qdisc_dev(sch);
++	struct dpaa2_eth_priv *priv_eth = netdev_priv(dev);
++	u8 ch_id = 0;
++	int err;
++
++	memset(&xstats, 0, sizeof(xstats));
++	memset(&tmp_bstats, 0, sizeof(tmp_bstats));
++
++	if (cl->type == CEETM_ROOT)
++		return 0;
++
++	err = dpni_get_statistics(priv_eth->mc_io, 0, priv_eth->mc_token, 3,
++				  DPNI_BUILD_CH_TC(ch_id, cl->prio.qpri),
++				  &dpni_stats);
++	if (err)
++		netdev_warn(dev, "dpni_get_stats(%d) failed - %d\n", 3, err);
++
++	xstats.ceetm_dequeue_bytes = dpni_stats.page_3.egress_dequeue_bytes;
++	xstats.ceetm_dequeue_frames = dpni_stats.page_3.egress_dequeue_frames;
++	xstats.ceetm_reject_bytes = dpni_stats.page_3.egress_reject_bytes;
++	xstats.ceetm_reject_frames = dpni_stats.page_3.egress_reject_frames;
++
++	return gnet_stats_copy_app(d, &xstats, sizeof(xstats));
++}
++
++static struct tcf_block *dpaa2_ceetm_tcf_block(struct Qdisc *sch,
++					       unsigned long arg,
++					       struct netlink_ext_ack *extack)
++{
++	struct dpaa2_ceetm_qdisc *priv = qdisc_priv(sch);
++	struct dpaa2_ceetm_class *cl = (struct dpaa2_ceetm_class *)arg;
++
++	pr_debug(KBUILD_BASENAME " : %s : class %X under qdisc %X\n", __func__,
++		 cl ? cl->common.classid : 0, sch->handle);
++	return cl ? cl->block : priv->block;
++}
++
++static unsigned long dpaa2_ceetm_tcf_bind(struct Qdisc *sch,
++					  unsigned long parent,
++					  u32 classid)
++{
++	struct dpaa2_ceetm_class *cl = dpaa2_ceetm_find(classid, sch);
++
++	pr_debug(KBUILD_BASENAME " : %s : class %X under qdisc %X\n", __func__,
++		 cl ? cl->common.classid : 0, sch->handle);
++	return (unsigned long)cl;
++}
++
++static void dpaa2_ceetm_tcf_unbind(struct Qdisc *sch, unsigned long arg)
++{
++	struct dpaa2_ceetm_class *cl = (struct dpaa2_ceetm_class *)arg;
++
++	pr_debug(KBUILD_BASENAME " : %s : class %X under qdisc %X\n", __func__,
++		 cl ? cl->common.classid : 0, sch->handle);
++}
++
++const struct Qdisc_class_ops dpaa2_ceetm_cls_ops = {
++	.graft		=	dpaa2_ceetm_cls_graft,
++	.leaf		=	dpaa2_ceetm_cls_leaf,
++	.find		=	dpaa2_ceetm_cls_find,
++	.change		=	dpaa2_ceetm_cls_change,
++	.delete		=	dpaa2_ceetm_cls_delete,
++	.walk		=	dpaa2_ceetm_cls_walk,
++	.tcf_block	=	dpaa2_ceetm_tcf_block,
++	.bind_tcf	=	dpaa2_ceetm_tcf_bind,
++	.unbind_tcf	=	dpaa2_ceetm_tcf_unbind,
++	.dump		=	dpaa2_ceetm_cls_dump,
++	.dump_stats	=	dpaa2_ceetm_cls_dump_stats,
++};
++
++struct Qdisc_ops dpaa2_ceetm_qdisc_ops __read_mostly = {
++	.id		=	"ceetm",
++	.priv_size	=	sizeof(struct dpaa2_ceetm_qdisc),
++	.cl_ops		=	&dpaa2_ceetm_cls_ops,
++	.init		=	dpaa2_ceetm_init,
++	.destroy	=	dpaa2_ceetm_destroy,
++	.change		=	dpaa2_ceetm_change,
++	.dump		=	dpaa2_ceetm_dump,
++	.attach		=	dpaa2_ceetm_attach,
++	.owner		=	THIS_MODULE,
++};
++
++/* Run the filters and classifiers attached to the qdisc on the provided skb */
++int dpaa2_ceetm_classify(struct sk_buff *skb, struct Qdisc *sch,
++			 int *qdid, u8 *qpri)
++{
++	struct dpaa2_ceetm_qdisc *priv = qdisc_priv(sch);
++	struct dpaa2_ceetm_class *cl = NULL;
++	struct tcf_result res;
++	struct tcf_proto *tcf;
++	int result;
++
++	tcf = rcu_dereference_bh(priv->filter_list);
++	while (tcf && (result = tcf_classify(skb, tcf, &res, false)) >= 0) {
++#ifdef CONFIG_NET_CLS_ACT
++		switch (result) {
++		case TC_ACT_QUEUED:
++		case TC_ACT_STOLEN:
++		case TC_ACT_SHOT:
++			/* No valid class found due to action */
++			return -1;
++		}
++#endif
++		cl = (void *)res.class;
++		if (!cl) {
++			/* The filter leads to the qdisc */
++			if (res.classid == sch->handle)
++				return 0;
++
++			cl = dpaa2_ceetm_find(res.classid, sch);
++			/* The filter leads to an invalid class */
++			if (!cl)
++				break;
++		}
++
++		/* The class might have its own filters attached */
++		tcf = rcu_dereference_bh(cl->filter_list);
++	}
++
++	/* No valid class found */
++	if (!cl)
++		return 0;
++
++	switch (cl->type) {
++	case CEETM_ROOT:
++		*qdid = cl->root.ch_id;
++
++		/* The root class does not have a child prio qdisc */
++		if (!cl->child)
++			return 0;
++
++		/* Run the prio qdisc classifiers */
++		return dpaa2_ceetm_classify(skb, cl->child, qdid, qpri);
++
++	case CEETM_PRIO:
++		*qpri = cl->prio.qpri;
++		break;
++	}
++
++	return 0;
++}
++
++int __init dpaa2_ceetm_register(void)
++{
++	int err = 0;
++
++	pr_debug(KBUILD_MODNAME ": " DPAA2_CEETM_DESCRIPTION "\n");
++
++	err = register_qdisc(&dpaa2_ceetm_qdisc_ops);
++	if (unlikely(err))
++		pr_err(KBUILD_MODNAME
++		       ": %s:%hu:%s(): register_qdisc() = %d\n",
++		       KBUILD_BASENAME ".c", __LINE__, __func__, err);
++
++	return err;
++}
++
++void __exit dpaa2_ceetm_unregister(void)
++{
++	pr_debug(KBUILD_MODNAME ": %s:%s() ->\n",
++		 KBUILD_BASENAME ".c", __func__);
++
++	unregister_qdisc(&dpaa2_ceetm_qdisc_ops);
++}
+--- /dev/null
++++ b/drivers/net/ethernet/freescale/dpaa2/dpaa2-eth-ceetm.h
+@@ -0,0 +1,207 @@
++/* Copyright 2017 NXP
++ *
++ * Redistribution and use in source and binary forms, with or without
++ * modification, are permitted provided that the following conditions are met:
++ *     * Redistributions of source code must retain the above copyright
++ *	 notice, this list of conditions and the following disclaimer.
++ *     * Redistributions in binary form must reproduce the above copyright
++ *	 notice, this list of conditions and the following disclaimer in the
++ *	 documentation and/or other materials provided with the distribution.
++ *     * Neither the name of Freescale Semiconductor nor the
++ *	 names of its contributors may be used to endorse or promote products
++ *	 derived from this software without specific prior written permission.
++ *
++ *
++ * ALTERNATIVELY, this software may be distributed under the terms of the
++ * GNU General Public License ("GPL") as published by the Free Software
++ * Foundation, either version 2 of that License or (at your option) any
++ * later version.
++ *
++ * THIS SOFTWARE IS PROVIDED BY Freescale Semiconductor ``AS IS'' AND ANY
++ * EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
++ * WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
++ * DISCLAIMED. IN NO EVENT SHALL Freescale Semiconductor BE LIABLE FOR ANY
++ * DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
++ * (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
++ * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
++ * ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
++ * (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
++ * SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
++ */
++
++#ifndef __DPAA2_ETH_CEETM_H
++#define __DPAA2_ETH_CEETM_H
++
++#include <net/pkt_sched.h>
++#include <net/pkt_cls.h>
++#include <net/netlink.h>
++
++#include "dpaa2-eth.h"
++
++/* For functional purposes, there are num_tx_queues pfifo qdiscs through which
++ * frames reach the driver. Their handles start from 1:21. Handles 1:1 to 1:20
++ * are reserved for the maximum 32 CEETM channels (majors and minors are in
++ * hex).
++ */
++#define PFIFO_MIN_OFFSET		0x21
++
++#define DPAA2_CEETM_MIN_WEIGHT		100
++#define DPAA2_CEETM_MAX_WEIGHT		24800
++
++#define DPAA2_CEETM_TD_THRESHOLD	1000
++
++enum wbfs_group_type {
++	WBFS_GRP_A,
++	WBFS_GRP_B,
++	WBFS_GRP_LARGE
++};
++
++enum {
++	DPAA2_CEETM_TCA_UNSPEC,
++	DPAA2_CEETM_TCA_COPT,
++	DPAA2_CEETM_TCA_QOPS,
++	DPAA2_CEETM_TCA_MAX,
++};
++
++/* CEETM configuration types */
++enum dpaa2_ceetm_type {
++	CEETM_ROOT = 1,
++	CEETM_PRIO,
++};
++
++enum {
++	STRICT_PRIORITY = 0,
++	WEIGHTED_A,
++	WEIGHTED_B,
++};
++
++struct dpaa2_ceetm_shaping_cfg {
++	__u64 cir; /* committed information rate */
++	__u64 eir; /* excess information rate */
++	__u16 cbs; /* committed burst size */
++	__u16 ebs; /* excess burst size */
++	__u8 coupled; /* shaper coupling */
++};
++
++extern const struct nla_policy ceetm_policy[DPAA2_CEETM_TCA_MAX];
++
++struct dpaa2_ceetm_class;
++struct dpaa2_ceetm_qdisc_stats;
++struct dpaa2_ceetm_class_stats;
++
++/* corresponds to CEETM shaping at LNI level */
++struct dpaa2_root_q {
++	struct Qdisc **qdiscs;
++	struct dpaa2_ceetm_qdisc_stats __percpu *qstats;
++};
++
++/* corresponds to the number of priorities a channel serves */
++struct dpaa2_prio_q {
++	struct dpaa2_ceetm_class *parent;
++	struct dpni_tx_priorities_cfg tx_prio_cfg;
++};
++
++struct dpaa2_ceetm_qdisc {
++	struct Qdisc_class_hash clhash;
++	struct tcf_proto *filter_list; /* qdisc attached filters */
++	struct tcf_block *block;
++
++	enum dpaa2_ceetm_type type; /* ROOT/PRIO */
++	bool shaped;
++	union {
++		struct dpaa2_root_q root;
++		struct dpaa2_prio_q prio;
++	};
++};
++
++/* CEETM Qdisc configuration parameters */
++struct dpaa2_ceetm_tc_qopt {
++	enum dpaa2_ceetm_type type;
++	__u16 shaped;
++	__u8 prio_group_A;
++	__u8 prio_group_B;
++	__u8 separate_groups;
++};
++
++/* root class - corresponds to a channel */
++struct dpaa2_root_c {
++	struct dpaa2_ceetm_shaping_cfg shaping_cfg;
++	u32 ch_id;
++};
++
++/* prio class - corresponds to a strict priority queue (group) */
++struct dpaa2_prio_c {
++	struct dpaa2_ceetm_class_stats __percpu *cstats;
++	u32 qpri;
++	u8 mode;
++	u16 weight;
++};
++
++struct dpaa2_ceetm_class {
++	struct Qdisc_class_common common;
++	struct tcf_proto *filter_list; /* class attached filters */
++	struct tcf_block *block;
++	struct Qdisc *parent;
++	struct Qdisc *child;
++
++	enum dpaa2_ceetm_type type; /* ROOT/PRIO */
++	bool shaped;
++	union {
++		struct dpaa2_root_c root;
++		struct dpaa2_prio_c prio;
++	};
++};
++
++/* CEETM Class configuration parameters */
++struct dpaa2_ceetm_tc_copt {
++	enum dpaa2_ceetm_type type;
++	struct dpaa2_ceetm_shaping_cfg shaping_cfg;
++	__u16 shaped;
++	__u8 mode;
++	__u16 weight;
++};
++
++/* CEETM stats */
++struct dpaa2_ceetm_qdisc_stats {
++	__u32 drops;
++};
++
++struct dpaa2_ceetm_class_stats {
++	/* Software counters */
++	struct gnet_stats_basic_packed bstats;
++	__u32 ern_drop_count;
++};
++
++struct dpaa2_ceetm_tc_xstats {
++	__u64 ceetm_dequeue_bytes;
++	__u64 ceetm_dequeue_frames;
++	__u64 ceetm_reject_bytes;
++	__u64 ceetm_reject_frames;
++};
++
++#ifdef CONFIG_FSL_DPAA2_ETH_CEETM
++int __init dpaa2_ceetm_register(void);
++void __exit dpaa2_ceetm_unregister(void);
++int dpaa2_ceetm_classify(struct sk_buff *skb, struct Qdisc *sch,
++			 int *qdid, u8 *qpri);
++#else
++static inline int dpaa2_ceetm_register(void)
++{
++	return 0;
++}
++
++static inline void dpaa2_ceetm_unregister(void) {}
++
++static inline int dpaa2_ceetm_classify(struct sk_buff *skb, struct Qdisc *sch,
++				       int *qdid, u8 *qpri)
++{
++	return 0;
++}
++#endif
++
++static inline bool dpaa2_eth_ceetm_is_enabled(struct dpaa2_eth_priv *priv)
++{
++	return priv->ceetm_en;
++}
++
++#endif
+--- a/drivers/net/ethernet/freescale/dpaa2/dpaa2-eth.c
++++ b/drivers/net/ethernet/freescale/dpaa2/dpaa2-eth.c
+@@ -18,6 +18,7 @@
+ #include <net/sock.h>
+ 
+ #include "dpaa2-eth.h"
++#include "dpaa2-eth-ceetm.h"
+ 
+ /* CREATE_TRACE_POINTS only needs to be defined once. Other dpa files
+  * using trace events only need to #include <trace/events/sched.h>
+@@ -816,7 +817,7 @@ static netdev_tx_t dpaa2_eth_tx(struct s
+ 	unsigned int needed_headroom;
+ 	u32 fd_len;
+ 	u8 prio = 0;
+-	int err, i;
++	int err, i, ch_id = 0;
+ 
+ 	percpu_stats = this_cpu_ptr(priv->percpu_stats);
+ 	percpu_extras = this_cpu_ptr(priv->percpu_extras);
+@@ -887,6 +888,15 @@ static netdev_tx_t dpaa2_eth_tx(struct s
+ 	}
+ 	fq = &priv->fq[queue_mapping];
+ 
++	if (dpaa2_eth_ceetm_is_enabled(priv)) {
++		err = dpaa2_ceetm_classify(skb, net_dev->qdisc, &ch_id, &prio);
++		if (err) {
++			free_tx_fd(priv, fq, &fd, false);
++			percpu_stats->tx_dropped++;
++			return NETDEV_TX_OK;
++		}
++	}
++
+ 	fd_len = dpaa2_fd_get_len(&fd);
+ 	nq = netdev_get_tx_queue(net_dev, queue_mapping);
+ 	netdev_tx_sent_queue(nq, fd_len);
+@@ -2075,17 +2085,13 @@ static int update_xps(struct dpaa2_eth_p
+ 	return err;
+ }
+ 
+-static int dpaa2_eth_setup_tc(struct net_device *net_dev,
+-			      enum tc_setup_type type, void *type_data)
++static int dpaa2_eth_setup_mqprio(struct net_device *net_dev,
++				  struct tc_mqprio_qopt *mqprio)
+ {
+ 	struct dpaa2_eth_priv *priv = netdev_priv(net_dev);
+-	struct tc_mqprio_qopt *mqprio = type_data;
+ 	u8 num_tc, num_queues;
+ 	int i;
+ 
+-	if (type != TC_SETUP_QDISC_MQPRIO)
+-		return -EINVAL;
+-
+ 	mqprio->hw = TC_MQPRIO_HW_OFFLOAD_TCS;
+ 	num_queues = dpaa2_eth_queue_count(priv);
+ 	num_tc = mqprio->num_tc;
+@@ -2117,6 +2123,20 @@ out:
+ 	return 0;
+ }
+ 
++static int dpaa2_eth_setup_tc(struct net_device *net_dev,
++			      enum tc_setup_type type,
++			      void *type_data)
++{
++	switch (type) {
++	case TC_SETUP_BLOCK:
++		return 0;
++	case TC_SETUP_QDISC_MQPRIO:
++		return dpaa2_eth_setup_mqprio(net_dev, type_data);
++	default:
++		return -EOPNOTSUPP;
++	}
++}
++
+ static const struct net_device_ops dpaa2_eth_ops = {
+ 	.ndo_open = dpaa2_eth_open,
+ 	.ndo_start_xmit = dpaa2_eth_tx,
+@@ -4166,18 +4186,27 @@ static int __init dpaa2_eth_driver_init(
+ 
+ 	dpaa2_eth_dbg_init();
+ 	err = fsl_mc_driver_register(&dpaa2_eth_driver);
+-	if (err) {
+-		dpaa2_eth_dbg_exit();
+-		return err;
+-	}
++	if (err)
++		goto out_debugfs_err;
++
++	err = dpaa2_ceetm_register();
++	if (err)
++		goto out_ceetm_err;
+ 
+ 	return 0;
++
++out_ceetm_err:
++	fsl_mc_driver_unregister(&dpaa2_eth_driver);
++out_debugfs_err:
++	dpaa2_eth_dbg_exit();
++	return err;
+ }
+ 
+ static void __exit dpaa2_eth_driver_exit(void)
+ {
+-	dpaa2_eth_dbg_exit();
++	dpaa2_ceetm_unregister();
+ 	fsl_mc_driver_unregister(&dpaa2_eth_driver);
++	dpaa2_eth_dbg_exit();
+ }
+ 
+ module_init(dpaa2_eth_driver_init);
+--- a/drivers/net/ethernet/freescale/dpaa2/dpaa2-eth.h
++++ b/drivers/net/ethernet/freescale/dpaa2/dpaa2-eth.h
+@@ -453,6 +453,8 @@ struct dpaa2_eth_priv {
+ 	struct dpaa2_debugfs dbg;
+ #endif
+ 	struct dpni_tx_shaping_cfg shaping_cfg;
++
++	bool ceetm_en;
+ };
+ 
+ #define DPAA2_RXH_SUPPORTED	(RXH_L2DA | RXH_VLAN | RXH_L3_PROTO \
+@@ -573,6 +575,11 @@ static inline unsigned int dpaa2_eth_rx_
+ 	return priv->tx_data_offset - DPAA2_ETH_RX_HWA_SIZE;
+ }
+ 
++static inline int dpaa2_eth_ch_count(struct dpaa2_eth_priv *priv)
++{
++	return 1;
++}
++
+ int dpaa2_eth_set_hash(struct net_device *net_dev, u64 flags);
+ int dpaa2_eth_set_cls(struct net_device *net_dev, u64 key);
+ int dpaa2_eth_cls_key_size(u64 key);