ASR_BASE
Change-Id: Icf3719cc0afe3eeb3edc7fa80a2eb5199ca9dda1
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 0000000..ca06007
--- /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 -EOPNOTSUPP;
+-
+ 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,
+@@ -4173,18 +4193,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
+@@ -454,6 +454,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 \
+@@ -574,6 +576,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);