cbs: Add support for the graft function

This will allow to install a child qdisc under cbs. The main use case
is to install ETF (Earliest TxTime First) qdisc under cbs, so there's
another level of control for time-sensitive traffic.

Signed-off-by: Vinicius Costa Gomes <vinicius.gomes@intel.com>
Signed-off-by: David S. Miller <davem@davemloft.net>

1 files changed, 125 insertions, 9 deletions

diff --git a/net/sched/sch_cbs.c b/net/sched/sch_cbs.c
index cdd96b9a27bc..e26a24017faa 100644
--- a/net/sched/sch_cbs.c
+++ b/net/sched/sch_cbs.c
@@ -78,18 +78,42 @@ struct cbs_sched_data {
         s64 sendslope; /* in bytes/s */
         s64 idleslope; /* in bytes/s */
         struct qdisc_watchdog watchdog;
-        int (*enqueue)(struct sk_buff *skb, struct Qdisc *sch);
+        int (*enqueue)(struct sk_buff *skb, struct Qdisc *sch,
+                       struct sk_buff **to_free);
         struct sk_buff *(*dequeue)(struct Qdisc *sch);
+        struct Qdisc *qdisc;
 };
 
-static int cbs_enqueue_offload(struct sk_buff *skb, struct Qdisc *sch)
+static int cbs_child_enqueue(struct sk_buff *skb, struct Qdisc *sch,
+                             struct Qdisc *child,
+                             struct sk_buff **to_free)
 {
-        return qdisc_enqueue_tail(skb, sch);
+        int err;
+
+        err = child->ops->enqueue(skb, child, to_free);
+        if (err != NET_XMIT_SUCCESS)
+                return err;
+
+        qdisc_qstats_backlog_inc(sch, skb);
+        sch->q.qlen++;
+
+        return NET_XMIT_SUCCESS;
 }
 
-static int cbs_enqueue_soft(struct sk_buff *skb, struct Qdisc *sch)
+static int cbs_enqueue_offload(struct sk_buff *skb, struct Qdisc *sch,
+                               struct sk_buff **to_free)
 {
         struct cbs_sched_data *q = qdisc_priv(sch);
+        struct Qdisc *qdisc = q->qdisc;
+
+        return cbs_child_enqueue(skb, sch, qdisc, to_free);
+}
+
+static int cbs_enqueue_soft(struct sk_buff *skb, struct Qdisc *sch,
+                            struct sk_buff **to_free)
+{
+        struct cbs_sched_data *q = qdisc_priv(sch);
+        struct Qdisc *qdisc = q->qdisc;
 
         if (sch->q.qlen == 0 && q->credits > 0) {
                 /* We need to stop accumulating credits when there's
@@ -99,7 +123,7 @@ static int cbs_enqueue_soft(struct sk_buff *skb, struct Qdisc *sch)
                 q->last = ktime_get_ns();
         }
 
-        return qdisc_enqueue_tail(skb, sch);
+        return cbs_child_enqueue(skb, sch, qdisc, to_free);
 }
 
 static int cbs_enqueue(struct sk_buff *skb, struct Qdisc *sch,
@@ -107,7 +131,7 @@ static int cbs_enqueue(struct sk_buff *skb, struct Qdisc *sch,
 {
         struct cbs_sched_data *q = qdisc_priv(sch);
 
-        return q->enqueue(skb, sch);
+        return q->enqueue(skb, sch, to_free);
 }
 
 /* timediff is in ns, slope is in bytes/s */
@@ -132,9 +156,25 @@ static s64 credits_from_len(unsigned int len, s64 slope, s64 port_rate)
         return div64_s64(len * slope, port_rate);
 }
 
+static struct sk_buff *cbs_child_dequeue(struct Qdisc *sch, struct Qdisc *child)
+{
+        struct sk_buff *skb;
+
+        skb = child->ops->dequeue(child);
+        if (!skb)
+                return NULL;
+
+        qdisc_qstats_backlog_dec(sch, skb);
+        qdisc_bstats_update(sch, skb);
+        sch->q.qlen--;
+
+        return skb;
+}
+
 static struct sk_buff *cbs_dequeue_soft(struct Qdisc *sch)
 {
         struct cbs_sched_data *q = qdisc_priv(sch);
+        struct Qdisc *qdisc = q->qdisc;
         s64 now = ktime_get_ns();
         struct sk_buff *skb;
         s64 credits;
@@ -157,8 +197,7 @@ static struct sk_buff *cbs_dequeue_soft(struct Qdisc *sch)
                         return NULL;
                 }
         }
-
-        skb = qdisc_dequeue_head(sch);
+        skb = cbs_child_dequeue(sch, qdisc);
         if (!skb)
                 return NULL;
 
@@ -178,7 +217,10 @@ static struct sk_buff *cbs_dequeue_soft(struct Qdisc *sch)
 
 static struct sk_buff *cbs_dequeue_offload(struct Qdisc *sch)
 {
-        return qdisc_dequeue_head(sch);
+        struct cbs_sched_data *q = qdisc_priv(sch);
+        struct Qdisc *qdisc = q->qdisc;
+
+        return cbs_child_dequeue(sch, qdisc);
 }
 
 static struct sk_buff *cbs_dequeue(struct Qdisc *sch)
@@ -310,6 +352,13 @@ static int cbs_init(struct Qdisc *sch, struct nlattr *opt,
                 return -EINVAL;
         }
 
+        q->qdisc = qdisc_create_dflt(sch->dev_queue, &pfifo_qdisc_ops,
+                                     sch->handle, extack);
+        if (!q->qdisc)
+                return -ENOMEM;
+
+        qdisc_hash_add(q->qdisc, false);
+
         q->queue = sch->dev_queue - netdev_get_tx_queue(dev, 0);
 
         q->enqueue = cbs_enqueue_soft;
@@ -328,6 +377,9 @@ static void cbs_destroy(struct Qdisc *sch)
         qdisc_watchdog_cancel(&q->watchdog);
 
         cbs_disable_offload(dev, q);
+
+        if (q->qdisc)
+                qdisc_destroy(q->qdisc);
 }
 
 static int cbs_dump(struct Qdisc *sch, struct sk_buff *skb)
@@ -356,8 +408,72 @@ nla_put_failure:
         return -1;
 }
 
+static int cbs_dump_class(struct Qdisc *sch, unsigned long cl,
+                          struct sk_buff *skb, struct tcmsg *tcm)
+{
+        struct cbs_sched_data *q = qdisc_priv(sch);
+
+        if (cl != 1 || !q->qdisc)       /* only one class */
+                return -ENOENT;
+
+        tcm->tcm_handle |= TC_H_MIN(1);
+        tcm->tcm_info = q->qdisc->handle;
+
+        return 0;
+}
+
+static int cbs_graft(struct Qdisc *sch, unsigned long arg, struct Qdisc *new,
+                     struct Qdisc **old, struct netlink_ext_ack *extack)
+{
+        struct cbs_sched_data *q = qdisc_priv(sch);
+
+        if (!new) {
+                new = qdisc_create_dflt(sch->dev_queue, &pfifo_qdisc_ops,
+                                        sch->handle, NULL);
+                if (!new)
+                        new = &noop_qdisc;
+        }
+
+        *old = qdisc_replace(sch, new, &q->qdisc);
+        return 0;
+}
+
+static struct Qdisc *cbs_leaf(struct Qdisc *sch, unsigned long arg)
+{
+        struct cbs_sched_data *q = qdisc_priv(sch);
+
+        return q->qdisc;
+}
+
+static unsigned long cbs_find(struct Qdisc *sch, u32 classid)
+{
+        return 1;
+}
+
+static void cbs_walk(struct Qdisc *sch, struct qdisc_walker *walker)
+{
+        if (!walker->stop) {
+                if (walker->count >= walker->skip) {
+                        if (walker->fn(sch, 1, walker) < 0) {
+                                walker->stop = 1;
+                                return;
+                        }
+                }
+                walker->count++;
+        }
+}
+
+static const struct Qdisc_class_ops cbs_class_ops = {
+        .graft          =       cbs_graft,
+        .leaf           =       cbs_leaf,
+        .find           =       cbs_find,
+        .walk           =       cbs_walk,
+        .dump           =       cbs_dump_class,
+};
+
 static struct Qdisc_ops cbs_qdisc_ops __read_mostly = {
         .id             =       "cbs",
+        .cl_ops         =       &cbs_class_ops,
         .priv_size      =       sizeof(struct cbs_sched_data),
         .enqueue        =       cbs_enqueue,
         .dequeue        =       cbs_dequeue,